From 7382d98b72dd839d19a874ce8af7f4552037e246 Mon Sep 17 00:00:00 2001 From: youngjae Date: Wed, 1 Nov 2023 01:29:52 -0400 Subject: [PATCH 1/4] delete ctrl package because can not build & add third party octomap server --- Ctrl/CMakeLists.txt | 29 - Ctrl/README.md | 1 - Ctrl/cmake/FindEigen.cmake | 173 --- Ctrl/config/ctrl_param_fpv.yaml | 1 - Ctrl/launch/ctrl_md.launch | 1 - Ctrl/package.xml | 18 - Ctrl/src/ctrl_node.cpp | 33 - third_party/octomap_server/CHANGELOG.rst | 250 ++++ third_party/octomap_server/CMakeLists.txt | 109 ++ .../octomap_server/cfg/OctomapServer.cfg | 26 + .../octomap_server/color_nodelet_plugins.xml | 7 + .../include/octomap_server/OctomapServer.h | 265 ++++ .../octomap_server/OctomapServerMultilayer.h | 73 + .../octomap_server/TrackingOctomapServer.h | 59 + .../launch/octomap_mapping.launch | 23 + .../launch/octomap_mapping_nodelet.launch | 28 + .../launch/octomap_tracking_client.launch | 13 + .../launch/octomap_tracking_server.launch | 19 + third_party/octomap_server/mainpage.dox | 30 + .../octomap_server/nodelet_plugins.xml | 8 + third_party/octomap_server/package.xml | 53 + .../octomap_server/params/default.yaml | 1 + .../scripts/octomap_eraser_cli.py | 31 + .../octomap_server/src/OctomapServer.cpp | 1298 +++++++++++++++++ .../src/OctomapServerMultilayer.cpp | 268 ++++ .../src/TrackingOctomapServer.cpp | 155 ++ .../octomap_server/src/octomap_saver.cpp | 124 ++ .../src/octomap_server_multilayer.cpp | 71 + .../src/octomap_server_node.cpp | 88 ++ .../src/octomap_server_nodelet.cpp | 69 + .../src/octomap_server_static.cpp | 156 ++ .../src/octomap_tracking_server_node.cpp | 59 + 32 files changed, 3283 insertions(+), 256 deletions(-) delete mode 100755 Ctrl/CMakeLists.txt delete mode 100755 Ctrl/README.md delete mode 100755 Ctrl/cmake/FindEigen.cmake delete mode 100755 Ctrl/config/ctrl_param_fpv.yaml delete mode 100755 Ctrl/launch/ctrl_md.launch delete mode 100755 Ctrl/package.xml delete mode 100644 Ctrl/src/ctrl_node.cpp create mode 100644 third_party/octomap_server/CHANGELOG.rst create mode 100644 third_party/octomap_server/CMakeLists.txt create mode 100755 third_party/octomap_server/cfg/OctomapServer.cfg create mode 100644 third_party/octomap_server/color_nodelet_plugins.xml create mode 100644 third_party/octomap_server/include/octomap_server/OctomapServer.h create mode 100644 third_party/octomap_server/include/octomap_server/OctomapServerMultilayer.h create mode 100644 third_party/octomap_server/include/octomap_server/TrackingOctomapServer.h create mode 100644 third_party/octomap_server/launch/octomap_mapping.launch create mode 100644 third_party/octomap_server/launch/octomap_mapping_nodelet.launch create mode 100644 third_party/octomap_server/launch/octomap_tracking_client.launch create mode 100644 third_party/octomap_server/launch/octomap_tracking_server.launch create mode 100644 third_party/octomap_server/mainpage.dox create mode 100644 third_party/octomap_server/nodelet_plugins.xml create mode 100644 third_party/octomap_server/package.xml create mode 100644 third_party/octomap_server/params/default.yaml create mode 100755 third_party/octomap_server/scripts/octomap_eraser_cli.py create mode 100644 third_party/octomap_server/src/OctomapServer.cpp create mode 100644 third_party/octomap_server/src/OctomapServerMultilayer.cpp create mode 100644 third_party/octomap_server/src/TrackingOctomapServer.cpp create mode 100644 third_party/octomap_server/src/octomap_saver.cpp create mode 100644 third_party/octomap_server/src/octomap_server_multilayer.cpp create mode 100644 third_party/octomap_server/src/octomap_server_node.cpp create mode 100644 third_party/octomap_server/src/octomap_server_nodelet.cpp create mode 100644 third_party/octomap_server/src/octomap_server_static.cpp create mode 100644 third_party/octomap_server/src/octomap_tracking_server_node.cpp diff --git a/Ctrl/CMakeLists.txt b/Ctrl/CMakeLists.txt deleted file mode 100755 index 9fcc63d..0000000 --- a/Ctrl/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(Ctrl) - -set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "-std=c++14") -set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g") - -find_package(catkin REQUIRED COMPONENTS - roscpp -) - -catkin_package( - - # INCLUDE_DIRS include - # LIBRARIES Ctrl - # CATKIN_DEPENDS roscpp rospy - # DEPENDS system_lib -) - -include_directories( - ${catkin_INCLUDE_DIRS} -) - -add_executable(Ctrl_node - src/Ctrl_node.cpp) - -target_link_libraries(Ctrl_node - ${catkin_LIBRARIES} -) \ No newline at end of file diff --git a/Ctrl/README.md b/Ctrl/README.md deleted file mode 100755 index 90426fd..0000000 --- a/Ctrl/README.md +++ /dev/null @@ -1 +0,0 @@ -# Ctrl \ No newline at end of file diff --git a/Ctrl/cmake/FindEigen.cmake b/Ctrl/cmake/FindEigen.cmake deleted file mode 100755 index d45fad7..0000000 --- a/Ctrl/cmake/FindEigen.cmake +++ /dev/null @@ -1,173 +0,0 @@ -# Ceres Solver - A fast non-linear least squares minimizer -# Copyright 2015 Google Inc. All rights reserved. -# http://ceres-solver.org/ -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# * Neither the name of Google Inc. nor the names of its contributors may be -# used to endorse or promote products derived from this software without -# specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: alexs.mac@gmail.com (Alex Stewart) -# - -# FindEigen.cmake - Find Eigen library, version >= 3. -# -# This module defines the following variables: -# -# EIGEN_FOUND: TRUE iff Eigen is found. -# EIGEN_INCLUDE_DIRS: Include directories for Eigen. -# -# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h -# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0 -# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0 -# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0 -# -# The following variables control the behaviour of this module: -# -# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to -# search for eigen includes, e.g: /timbuktu/eigen3. -# -# The following variables are also defined by this module, but in line with -# CMake recommended FindPackage() module style should NOT be referenced directly -# by callers (use the plural variables detailed above instead). These variables -# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which -# are NOT re-called (i.e. search for library is not repeated) if these variables -# are set with valid values _in the CMake cache_. This means that if these -# variables are set directly in the cache, either by the user in the CMake GUI, -# or by the user passing -DVAR=VALUE directives to CMake when called (which -# explicitly defines a cache variable), then they will be used verbatim, -# bypassing the HINTS variables and other hard-coded search locations. -# -# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the -# include directory of any dependencies. - -# Called if we failed to find Eigen or any of it's required dependencies, -# unsets all public (designed to be used externally) variables and reports -# error message at priority depending upon [REQUIRED/QUIET/] argument. -macro(EIGEN_REPORT_NOT_FOUND REASON_MSG) - unset(EIGEN_FOUND) - unset(EIGEN_INCLUDE_DIRS) - # Make results of search visible in the CMake GUI if Eigen has not - # been found so that user does not have to toggle to advanced view. - mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR) - # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() - # use the camelcase library name, not uppercase. - if (Eigen_FIND_QUIETLY) - message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) - elseif (Eigen_FIND_REQUIRED) - message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN}) - else() - # Neither QUIETLY nor REQUIRED, use no priority which emits a message - # but continues configuration and allows generation. - message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN}) - endif () - return() -endmacro(EIGEN_REPORT_NOT_FOUND) - -# Protect against any alternative find_package scripts for this library having -# been called previously (in a client project) which set EIGEN_FOUND, but not -# the other variables we require / set here which could cause the search logic -# here to fail. -unset(EIGEN_FOUND) - -# Search user-installed locations first, so that we prefer user installs -# to system installs where both exist. -# -# TODO: Add standard Windows search locations for Eigen. -list(APPEND EIGEN_CHECK_INCLUDE_DIRS - /usr/local/include - /usr/local/homebrew/include # Mac OS X - /opt/local/var/macports/software # Mac OS X. - /opt/local/include - /usr/include) -# Additional suffixes to try appending to each search path. -list(APPEND EIGEN_CHECK_PATH_SUFFIXES - eigen3 # Default root directory for Eigen. - Eigen/include/eigen3 ) # Windows (for C:/Program Files prefix). - -# Search supplied hint directories first if supplied. -find_path(EIGEN_INCLUDE_DIR - NAMES Eigen/Core - PATHS ${EIGEN_INCLUDE_DIR_HINTS} - ${EIGEN_CHECK_INCLUDE_DIRS} - PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES}) - -if (NOT EIGEN_INCLUDE_DIR OR - NOT EXISTS ${EIGEN_INCLUDE_DIR}) - eigen_report_not_found( - "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to " - "path to eigen3 include directory, e.g. /usr/local/include/eigen3.") -endif (NOT EIGEN_INCLUDE_DIR OR - NOT EXISTS ${EIGEN_INCLUDE_DIR}) - -# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets -# if called. -set(EIGEN_FOUND TRUE) - -# Extract Eigen version from Eigen/src/Core/util/Macros.h -if (EIGEN_INCLUDE_DIR) - set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h) - if (NOT EXISTS ${EIGEN_VERSION_FILE}) - eigen_report_not_found( - "Could not find file: ${EIGEN_VERSION_FILE} " - "containing version information in Eigen install located at: " - "${EIGEN_INCLUDE_DIR}.") - else (NOT EXISTS ${EIGEN_VERSION_FILE}) - file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS) - - string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+" - EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") - string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1" - EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}") - - string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+" - EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") - string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1" - EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}") - - string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+" - EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}") - string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1" - EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}") - - # This is on a single line s/t CMake does not interpret it as a list of - # elements and insert ';' separators which would result in 3.;2.;0 nonsense. - set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}") - endif (NOT EXISTS ${EIGEN_VERSION_FILE}) -endif (EIGEN_INCLUDE_DIR) - -# Set standard CMake FindPackage variables if found. -if (EIGEN_FOUND) - set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) -endif (EIGEN_FOUND) - -# Handle REQUIRED / QUIET optional arguments and version. -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(Eigen - REQUIRED_VARS EIGEN_INCLUDE_DIRS - VERSION_VAR EIGEN_VERSION) - -# Only mark internal variables as advanced if we found Eigen, otherwise -# leave it visible in the standard GUI for the user to set manually. -if (EIGEN_FOUND) - mark_as_advanced(FORCE EIGEN_INCLUDE_DIR) -endif (EIGEN_FOUND) diff --git a/Ctrl/config/ctrl_param_fpv.yaml b/Ctrl/config/ctrl_param_fpv.yaml deleted file mode 100755 index 00d2afd..0000000 --- a/Ctrl/config/ctrl_param_fpv.yaml +++ /dev/null @@ -1 +0,0 @@ -# place-holder \ No newline at end of file diff --git a/Ctrl/launch/ctrl_md.launch b/Ctrl/launch/ctrl_md.launch deleted file mode 100755 index cddac8c..0000000 --- a/Ctrl/launch/ctrl_md.launch +++ /dev/null @@ -1 +0,0 @@ - \ No newline at end of file diff --git a/Ctrl/package.xml b/Ctrl/package.xml deleted file mode 100755 index 7af314e..0000000 --- a/Ctrl/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - Ctrl - 0.0.0 - The Ctrl package - todo - - - - - TODO - - - - - - - \ No newline at end of file diff --git a/Ctrl/src/ctrl_node.cpp b/Ctrl/src/ctrl_node.cpp deleted file mode 100644 index 3430da4..0000000 --- a/Ctrl/src/ctrl_node.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include "ros/ros.h" - -FiniteStateMachine* pFSM; //# https://www.youtube.com/watch?v=jaitqSU2HIA&ab_channel=BRicey - -int main(int argc, char* argv[]) { - ros::init(argc, argv, "Ctrl"); - ros::NodeHandle nh("~"); - - //# 쓸데없는 약어는 지양 - ros::Subscriber sub_odometry = nh.subscribe("odometry", - 100, - boost::bind(&Odom_Data_t::feed, &fsm.odom_data, _1), - ros::VoidConstPtr(), - ros::TransportHints().tcpNoDelay()); - - ros::Subscriber sub_trajectory = nh.subscribe("trajectory", - 100, - boost::bind(&Command_Data_t::feed, &fsm.cmd_data, _1), - ros::VoidConstPtr(),//# ??? - ros::TransportHints().tcpNoDelay()); - - ros::Subscriber sub_imu = nh.subscribe("imu", - 100, - boost::bind(&Imu_Data_t::feed, &fsm.imu_data, _1), - ros::VoidConstPtr(), - ros::TransportHints().tcpNoDelay()); - while (ros::ok()) { - - - } - - return 0; -} diff --git a/third_party/octomap_server/CHANGELOG.rst b/third_party/octomap_server/CHANGELOG.rst new file mode 100644 index 0000000..d94874a --- /dev/null +++ b/third_party/octomap_server/CHANGELOG.rst @@ -0,0 +1,250 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package octomap_server +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.6.5 (2020-04-23) +------------------ +* Add color server nodelet (`#68 `_, `#67 `_) +* Updated maintainer email +* Contributors: clunietp, Wolfgang Merkt + +0.6.4 (2020-01-08) +------------------ +* Add private node handle to fix nodelet support (`#61 `_), fixes `#39 `_ +* Add octomap_server_color library by default (`#60 `_) - by Matthew Powelson +* Check if part of a voxel is in occupancy range (`#59 `_) - by Jasper v. B. +* Contributors: Matthew Powelson, Wolfgang Merkt, Jasper v. B. + +0.6.3 (2019-01-28) +------------------ +* Fix compilation on Debian Stretch +* Get rgb from point cloud iterator without byte shift +* Contributors: Kentaro Wada, Wolfgang Merkt + +0.6.2 (2019-01-27) +------------------ +* Update maintainer email (Wolfgang Merkt) +* Change catkin_package `DEPENDS` to `OCTOMAP` to avoid CMake warning +* Update maintainer email (Arming Hornung) +* Update to use non deprecated pluginlib macro +* Fixed memory leak of colors pointer if COLOR_OCTOMAP_SERVER defined +* Contributors: Armin Hornung, Mikael Arguedas, Ronky, Wolfgang Merkt + +0.6.1 (2016-10-19) +------------------ +* Fix for Colored Octomap: Use PCLPoint everywhere + Fixes compiler error when enabling the define + for color. +* Fixed maxRange bug in OctomapServer.cpp for clearing +* Adjust maintainer email +* Contributors: Armin Hornung, Brandon Kinman, Felix Endres + +0.6.0 (2016-03-25) +------------------ +* Add sensor model parameters to dynamic_reconfigure +* Load map file from rosparam +* Add x and y filter for pointcloud +* Preparations for ColorOctomapServer (compile option, from source) +* Fix iterator in OctomapServer +* TrackingOctomapServer: Publish node center rather than index, prevent from publishing empty cloud +* Contributors: Armin Hornung, Javier V. Gomez, JJeremie Deray, MasakiMurooka, Shohei Fujii, Wolfgang Merkt + +0.5.3 (2014-01-09) +------------------ +* Fixing PCL linking errors on build farm + +0.5.2 (2014-01-09) +------------------ +* Fixing PCL linking errors on build farm + +0.5.1 (2013-11-25) +------------------ +* Fix missing nodelet plugin from install + +0.5.0 (2013-10-24) +------------------ +* Small fix in octomap_server_static usage +* Catkinization, remove support for arm_navigation + +0.4.9 (2013-06-27) +------------------ +* cleanup of unused functions +* Parameters, reading .bt files in octomap_server_static +* added simple octomap_server_static node to serve OctoMaps from .ot files (no scan integration) +* Fix for incremental 2D projection map updates (thx to B.Coenen for the report) +* Publish free space as MarkerArray and CollisionMap (set parameter ~publish_free_space=True to enable). Thx to I. Wieser! +* renamed OctomapServer's NodeHandle constructor parameter to be more clear, added the same to OctomapServerMultilayer + +0.4.8 (2013-01-08) +------------------ +* Applied patch from issue `#7 `_: Nodelet version of octomap_server + Modified to not change the global namespace +* fixes for cmake / catkin +* fixed octomap_server for OctoMap 1.5 (deprecations), adjusted to new msg format +* changed message format to contain only data, meta information stored in new message fields (untested for Groovy) + +0.4.6 (2013-01-28) +------------------ +* Added NodeHandle parameter to OctomapServerMultilayer constructor +* Commited patch `#7 `_, contributed by M. Liebhardt: Nodelet version of the octomap server +* octomap_server and octomap_saver now fully support both binary and full occupancy maps +* octomap_server can now open .ot files properly, updated octomap_ros to new-style stack.xml +* deprecated OctomapROS in octomap_ros => directly use octomap lib and conversions.h +* removed OctomapROS wrapper from octomap_server classes +* octomap_server manifest exports dynamic_reconfigure path in cppflags +* parameter in launch file adjusted + +0.4.5 (2012-06-18) +------------------ +* new parameter to enable incremental 2D mapping (experimental, default: false) +* bug fix for OctomapServer map projection +* Fixed OctomapServer not clearing obstacles in projected 2D map properly +* fixed map reset and incremental 2D updates +* added arm layer height lookup +* Fixed resolution change (dynamic_reconfigure) and dynamic map size w. incremental updates +* incremental update of projected 2D maps only in updated 3D region, map dynamically grows +* increased Electric compatibility of octomap_server +* OctomapServer keeps track of update region for downprojected 2D map + +0.4.4 (2012-04-20) +------------------ +* Turned octomap_msgs and octomap_ros into unary stacks, code in octomap_mapping adjusted + +0.4.3 (2012-04-17) +------------------ +* Merged rev 2477:2613 from trunk: + - fixed ground filter + - added missing license headers, improved code layout to ROS standard + - adjusted to OctoMap 1.4 changes + - collision map publisher & eraser script ported from branch + - disabled lazy update temporarily (needs param) + - dynamic reconfigure interface to limit query depth (and voxel resolution) on the fly + +0.4.2 (2012-03-16) +------------------ +* fixed ground filter (from trunk, electric)\nVersion increased to 0.4.2 + +0.4.1 (2012-02-21 16:50) +------------------------ +* switched octomap_ros and octomap_server to pure CMake-style linking, version 0.4.1 +* removed uneccesary FindEigen.cmake files + +0.4.0 (2012-02-21 15:37) +------------------------ +* removed eigen package from depends +* Transitioned octomap package to deprecated, now forwards flags with pkg-config to system dependency + +0.3.8 (2012-04-26) +------------------ +* increased octomap version to 1.4.2, stack version 0.3.8 + +0.3.7 (2012-02-22) +------------------ +* removed temp. workaround for unstable (Eigen for PCL included), increased stack version to 0.3.7 +* server/client architecture for octomap_server +* octomap_server: ground plane filter defaults to false, base_footprint frame now only required when filtering + +0.3.6 (2012-01-09) +------------------ +* changed to Eigen rosdep for electric and fuerte + +0.3.5 (2011-10-30) +------------------ +* added OctomapServerMultilayer as stub +* More refactoring of octomap_server, added hooks for node traversal +* OctomapServerCombined is now OctomapServer +* cleanup of octomap_server +* - adjusted octomap_mapping trunk to compile against ROS electric (only affects octomap_server). + => use branch for diamondback! + +0.3.4 (2011-10-12) +------------------ +* publish empty map (+vis) after reset +* OctomapServerCombined: Drop old octree completely when resetting +* OctomapServerCombined: Parameter for latching topics, reset service +* added srv and service implementation to clear a bbx region in OctomapServerCombined +* OctomapServer: + private -> protected + added default constructor +* octomap_server: + - fixed 2D map for larger volumes + - now handles an initial file always as static, topics are published latched then +* removed debug PCD writing +* - ground filter now more reliable, filtering in base frame of robot instead of global frame. + - more parameters for ground filter + +0.3.3 (2011-08-17 07:41) +------------------------ +* octomap package udpate to use new OctoMap 1.2 library only (no visualization). Removed dependency on Qt / QGLViewer. +* fixed ground plane appearing as occupied + +0.3.2 (2011-08-09) +------------------ +* merged in changes of octomap_mapping trunk (up to rev 1781): + - octomap updated to 1.1.1 (testing), tarball URL on ros.org + - ground plane extraction OctomapServerCombined, configurable using PCL + - fixes and cleanup in OctomapServerCombined +* parameters for ground plane filtering +* Ground plane extraction improved +* Ground plane extraction (pcl) for testing +* - octomap: use OctoMap 1.1.1 (testing) + - octomap_server: handle larger pruned nodes in 2D map projection +* refactoring & cleanup of OctomapServerCombined, ready for ground plane extraction +* merged back octomap_server from experimental branch: + - proper class with more capabilities + - now sends out map in various representations / visualizations + - subscribes to PointCloud2 with tf::MessageFilter + - uses octomap_ros wrapper / conversions + - OctomapServerCombined (experimental): also builds downprojected 2D map +* added MoveMap.msg from octomap2, extended conversions.h +* templated octomapMsg conversion functions +* octomap_saver adjusted to moved locations +* Moved messages and conversions to octomap_ros from octomap_server +* Removed unnecessary exports in manifests +* - fixes in mainfest / stack.xml for ROS 1.3 + - doxygen properly configured with rosdoc + - stack release 0.1.2 prep +* Preparations for .deb releases +* License in cpp files, restored compatibility with boxturtle +* Adjusted license to BSD, more parameters in octomap_server +* OctoMap server (copied from octomap repo, trunk) +* Initial checkin of octomap stack (nearly empty at the moment) + +0.3.1 (2011-07-15) +------------------ +* Patched for arm_navigation changes in "unstable" + +0.3.0 (2011-06-28) +------------------ +* merged back octomap_server from experimental branch: + - proper class with more capabilities + - now sends out map in various representations / visualizations + - subscribes to PointCloud2 with tf::MessageFilter + - uses octomap_ros wrapper / conversions + - OctomapServerCombined (experimental): also builds downprojected 2D map +* added MoveMap.msg from octomap2, extended conversions.h +* templated octomapMsg conversion functions + +0.2.0 (2011-03-16) +------------------ +* updated stack.xml for cturtle only +* octomap_saver adjusted to moved locations +* Moved messages and conversions to octomap_ros from octomap_server +* Removed unnecessary exports in manifests + +0.1.2 (2010-11-23) +------------------ +* - fixes in mainfest / stack.xml for ROS 1.3 + - doxygen properly configured with rosdoc + - stack release 0.1.2 prep + +0.1.1 (2010-11-17) +------------------ + +0.1.0 (2010-11-16) +------------------ +* Preparations for .deb releases +* License in cpp files, restored compatibility with boxturtle +* Adjusted license to BSD, more parameters in octomap_server +* OctoMap server (copied from octomap repo, trunk) +* Initial checkin of octomap stack (nearly empty at the moment) diff --git a/third_party/octomap_server/CMakeLists.txt b/third_party/octomap_server/CMakeLists.txt new file mode 100644 index 0000000..a9c8da3 --- /dev/null +++ b/third_party/octomap_server/CMakeLists.txt @@ -0,0 +1,109 @@ +cmake_minimum_required(VERSION 2.8) +project(octomap_server) + +set(PACKAGE_DEPENDENCIES + roscpp + visualization_msgs + sensor_msgs + pcl_ros + pcl_conversions + nav_msgs + std_msgs + std_srvs + octomap_ros + octomap_msgs + dynamic_reconfigure + nodelet +) + + +find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES}) + +find_package(octomap REQUIRED) +add_definitions(-DOCTOMAP_NODEBUGOUT) + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${OCTOMAP_INCLUDE_DIRS} +) + +generate_dynamic_reconfigure_options(cfg/OctomapServer.cfg) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS ${PACKAGE_DEPENDENCIES} + DEPENDS OCTOMAP +) + +set(LINK_LIBS + ${OCTOMAP_LIBRARIES} + ${catkin_LIBRARIES} +) + +add_library(${PROJECT_NAME} src/OctomapServer.cpp src/OctomapServerMultilayer.cpp src/TrackingOctomapServer.cpp) +target_link_libraries(${PROJECT_NAME} ${LINK_LIBS}) +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) + +add_library(${PROJECT_NAME}_color src/OctomapServer.cpp src/OctomapServerMultilayer.cpp src/TrackingOctomapServer.cpp) +target_link_libraries(${PROJECT_NAME}_color ${LINK_LIBS}) +add_dependencies(${PROJECT_NAME}_color ${PROJECT_NAME}_gencfg) +target_compile_definitions(${PROJECT_NAME}_color PUBLIC COLOR_OCTOMAP_SERVER) + +add_executable(octomap_server_node src/octomap_server_node.cpp) +target_link_libraries(octomap_server_node ${PROJECT_NAME} ${LINK_LIBS}) + +add_executable(octomap_color_server_node src/octomap_server_node.cpp) +target_link_libraries(octomap_color_server_node ${PROJECT_NAME}_color ${LINK_LIBS}) + +add_executable(octomap_server_static src/octomap_server_static.cpp) +target_link_libraries(octomap_server_static ${PROJECT_NAME} ${LINK_LIBS}) + +add_executable(octomap_server_multilayer src/octomap_server_multilayer.cpp) +target_link_libraries(octomap_server_multilayer ${PROJECT_NAME} ${LINK_LIBS}) + +add_executable(octomap_saver src/octomap_saver.cpp) +target_link_libraries(octomap_saver ${PROJECT_NAME} ${LINK_LIBS}) + +add_executable(octomap_tracking_server_node src/octomap_tracking_server_node.cpp) +target_link_libraries(octomap_tracking_server_node ${PROJECT_NAME} ${LINK_LIBS}) + +# Nodelet +add_library(octomap_server_nodelet src/octomap_server_nodelet.cpp) +target_link_libraries(octomap_server_nodelet ${PROJECT_NAME} ${LINK_LIBS}) + +add_library(octomap_color_server_nodelet src/octomap_server_nodelet.cpp) +target_link_libraries(octomap_color_server_nodelet ${PROJECT_NAME}_color ${LINK_LIBS}) + +# install targets: +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_color + octomap_server_node + octomap_color_server_node + octomap_server_static + octomap_server_multilayer + octomap_saver + octomap_tracking_server_node + octomap_server_nodelet + octomap_color_server_nodelet + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE +) + +install(FILES + nodelet_plugins.xml + color_nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/third_party/octomap_server/cfg/OctomapServer.cfg b/third_party/octomap_server/cfg/OctomapServer.cfg new file mode 100755 index 0000000..025f03a --- /dev/null +++ b/third_party/octomap_server/cfg/OctomapServer.cfg @@ -0,0 +1,26 @@ +#!/usr/bin/env python +PACKAGE = "octomap_server" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("compress_map", bool_t, 0, "Compresses the map losslessly", True) +gen.add("incremental_2D_projection", bool_t, 0, "Incremental 2D projection", False) +gen.add("filter_speckles", bool_t, 0, "Filter speckle nodes (with no neighbors)", False) +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) +gen.add("pointcloud_min_z", double_t, 0, "Minimum height of points to consider for insertion", -100, -100, 100) +gen.add("pointcloud_max_z", double_t, 0, "Maximum height of points to consider for insertion", 100, -100, 100) +gen.add("occupancy_min_z", double_t, 0, "Minimum height of occupied cells to consider in the final map", -100, -100, 100) +gen.add("occupancy_max_z", double_t, 0, "Maximum height of occupied cells to consider in the final map", 100, -100, 100) +gen.add("sensor_model_max_range", double_t, 0, "Sensor maximum range", -1.0, -1.0, 100) +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) +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) +gen.add("sensor_model_min", double_t, 0, "Minimum probability for clamping when dynamically building a map", 0.12, 0.0, 1.0) +gen.add("sensor_model_max", double_t, 0, "Maximum probability for clamping when dynamically building a map", 0.97, 0.0, 1.0) +gen.add("filter_ground", bool_t, 0, "Filter ground plane", False) +gen.add("ground_filter_distance", double_t, 0, "Distance threshold to consider a point as ground", 0.04, 0.001, 1) +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) +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) + +exit(gen.generate(PACKAGE, "octomap_server_node", "OctomapServer")) diff --git a/third_party/octomap_server/color_nodelet_plugins.xml b/third_party/octomap_server/color_nodelet_plugins.xml new file mode 100644 index 0000000..ef0ba54 --- /dev/null +++ b/third_party/octomap_server/color_nodelet_plugins.xml @@ -0,0 +1,7 @@ + + + + Nodelet for running the color Octomap server + + + \ No newline at end of file diff --git a/third_party/octomap_server/include/octomap_server/OctomapServer.h b/third_party/octomap_server/include/octomap_server/OctomapServer.h new file mode 100644 index 0000000..3edfaed --- /dev/null +++ b/third_party/octomap_server/include/octomap_server/OctomapServer.h @@ -0,0 +1,265 @@ +/* + * Copyright (c) 2010-2013, A. Hornung, University of Freiburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef OCTOMAP_SERVER_OCTOMAPSERVER_H +#define OCTOMAP_SERVER_OCTOMAPSERVER_H + +#include +#include +#include +#include + +// #include +// #include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +// 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 +// #define COLOR_OCTOMAP_SERVER + +#ifdef COLOR_OCTOMAP_SERVER +#include +#endif + +namespace octomap_server { +class OctomapServer { + +public: +#ifdef COLOR_OCTOMAP_SERVER + typedef pcl::PointXYZRGB PCLPoint; + typedef pcl::PointCloud PCLPointCloud; + typedef octomap::ColorOcTree OcTreeT; +#else + typedef pcl::PointXYZ PCLPoint; + typedef pcl::PointCloud PCLPointCloud; + typedef octomap::OcTree OcTreeT; +#endif + typedef octomap_msgs::GetOctomap OctomapSrv; + typedef octomap_msgs::BoundingBoxQuery BBXSrv; + + OctomapServer(const ros::NodeHandle private_nh_ = ros::NodeHandle("~"),const ros::NodeHandle &nh_ = ros::NodeHandle(),const std::string frame_id = "world_enu"); + virtual ~OctomapServer(); + virtual bool octomapBinarySrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res); + virtual bool octomapFullSrv(OctomapSrv::Request &req, OctomapSrv::GetOctomap::Response &res); + bool clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp); + bool resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp); + + virtual void insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud); + virtual bool openFile(const std::string& filename); + +// protected: + inline static void updateMinKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& min) { + for (unsigned i = 0; i < 3; ++i) + min[i] = std::min(in[i], min[i]); + }; + + inline static void updateMaxKey(const octomap::OcTreeKey& in, octomap::OcTreeKey& max) { + for (unsigned i = 0; i < 3; ++i) + max[i] = std::max(in[i], max[i]); + }; + + /// Test if key is within update area of map (2D, ignores height) + inline bool isInUpdateBBX(const OcTreeT::iterator& it) const { + // 2^(tree_depth-depth) voxels wide: + unsigned voxelWidth = (1 << (m_maxTreeDepth - it.getDepth())); + octomap::OcTreeKey key = it.getIndexKey(); // lower corner of voxel + return (key[0] + voxelWidth >= m_updateBBXMin[0] + && key[1] + voxelWidth >= m_updateBBXMin[1] + && key[0] <= m_updateBBXMax[0] + && key[1] <= m_updateBBXMax[1]); + } + + void reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level); + void publishBinaryOctoMap(const ros::Time& rostime = ros::Time::now()) const; + void publishFullOctoMap(const ros::Time& rostime = ros::Time::now()) const; + virtual void publishAll(const ros::Time& rostime = ros::Time::now()); + void publishMaker(const ros::Time& rostime); + + /** + * @brief update occupancy map with a scan labeled as ground and nonground. + * The scans should be in the global map frame. + * + * @param sensorOrigin origin of the measurements for raycasting + * @param ground scan endpoints on the ground plane (only clear space) + * @param nonground all other endpoints (clear up to occupied endpoint) + */ + virtual void insertScan(const tf::Point& sensorOrigin, const PCLPointCloud& ground, const PCLPointCloud& nonground); + + /// label the input cloud "pc" into ground and nonground. Should be in the robot's fixed frame (not world!) + void filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const; + + /** + * @brief Find speckle nodes (single occupied voxels with no neighbors). Only works on lowest resolution! + * @param key + * @return + */ + bool isSpeckleNode(const octomap::OcTreeKey& key) const; + + /// hook that is called before traversing all nodes + virtual void handlePreNodeTraversal(const ros::Time& rostime); + + /// hook that is called when traversing all nodes of the updated Octree (does nothing here) + virtual void handleNode(const OcTreeT::iterator& it) {}; + + /// hook that is called when traversing all nodes of the updated Octree in the updated area (does nothing here) + virtual void handleNodeInBBX(const OcTreeT::iterator& it) {}; + + /// hook that is called when traversing occupied nodes of the updated Octree + virtual void handleOccupiedNode(const OcTreeT::iterator& it); + + /// hook that is called when traversing occupied nodes in the updated area (updates 2D map projection here) + virtual void handleOccupiedNodeInBBX(const OcTreeT::iterator& it); + + /// hook that is called when traversing free nodes of the updated Octree + virtual void handleFreeNode(const OcTreeT::iterator& it); + + /// hook that is called when traversing free nodes in the updated area (updates 2D map projection here) + virtual void handleFreeNodeInBBX(const OcTreeT::iterator& it); + + /// hook that is called after traversing all nodes + virtual void handlePostNodeTraversal(const ros::Time& rostime); + + /// updates the downprojected 2D map as either occupied or free + virtual void update2DMap(const OcTreeT::iterator& it, bool occupied); + + inline unsigned mapIdx(int i, int j) const { + return m_gridmap.info.width * j + i; + } + + inline unsigned mapIdx(const octomap::OcTreeKey& key) const { + return mapIdx((key[0] - m_paddedMinKey[0]) / m_multires2DScale, + (key[1] - m_paddedMinKey[1]) / m_multires2DScale); + + } + + /** + * Adjust data of map due to a change in its info properties (origin or size, + * resolution needs to stay fixed). map already contains the new map info, + * but the data is stored according to oldMapInfo. + */ + + void adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const; + + inline bool mapChanged(const nav_msgs::MapMetaData& oldMapInfo, const nav_msgs::MapMetaData& newMapInfo) { + return ( oldMapInfo.height != newMapInfo.height + || oldMapInfo.width != newMapInfo.width + || oldMapInfo.origin.position.x != newMapInfo.origin.position.x + || oldMapInfo.origin.position.y != newMapInfo.origin.position.y); + } + + static std_msgs::ColorRGBA heightMapColor(double h); + ros::NodeHandle m_nh; + ros::NodeHandle m_nh_private; + ros::Publisher m_markerPub, m_binaryMapPub, m_fullMapPub, m_pointCloudPub, m_collisionObjectPub, m_mapPub, m_cmapPub, m_fmapPub, m_fmarkerPub; + message_filters::Subscriber* m_pointCloudSub; + tf::MessageFilter* m_tfPointCloudSub; + ros::ServiceServer m_octomapBinaryService, m_octomapFullService, m_clearBBXService, m_resetService; + tf::TransformListener m_tfListener; + boost::recursive_mutex m_config_mutex; + dynamic_reconfigure::Server m_reconfigureServer; + + OcTreeT* m_octree; + octomap::KeyRay m_keyRay; // temp storage for ray casting + octomap::OcTreeKey m_updateBBXMin; + octomap::OcTreeKey m_updateBBXMax; + + double m_maxRange; + std::string m_worldFrameId; // the map frame + std::string m_baseFrameId; // base of the robot for ground plane filtering + bool m_useHeightMap; + std_msgs::ColorRGBA m_color; + std_msgs::ColorRGBA m_colorFree; + double m_colorFactor; + + bool m_latchedTopics; + bool m_publishFreeSpace; + + double m_res; + unsigned m_treeDepth; + unsigned m_maxTreeDepth; + + double m_pointcloudMinX; + double m_pointcloudMaxX; + double m_pointcloudMinY; + double m_pointcloudMaxY; + double m_pointcloudMinZ; + double m_pointcloudMaxZ; + double m_occupancyMinZ; + double m_occupancyMaxZ; + double m_minSizeX; + double m_minSizeY; + bool m_filterSpeckles; + + bool m_filterGroundPlane; + double m_groundFilterDistance; + double m_groundFilterAngle; + double m_groundFilterPlaneDistance; + + bool m_compressMap; + + bool m_initConfig; + + // downprojected 2D map: + bool m_incrementalUpdate; + nav_msgs::OccupancyGrid m_gridmap; + bool m_publish2DMap; + bool m_mapOriginChanged; + octomap::OcTreeKey m_paddedMinKey; + unsigned m_multires2DScale; + bool m_projectCompleteMap; + bool m_useColoredMap; +}; +} + +#endif diff --git a/third_party/octomap_server/include/octomap_server/OctomapServerMultilayer.h b/third_party/octomap_server/include/octomap_server/OctomapServerMultilayer.h new file mode 100644 index 0000000..0449c47 --- /dev/null +++ b/third_party/octomap_server/include/octomap_server/OctomapServerMultilayer.h @@ -0,0 +1,73 @@ +/* + * Copyright (c) 2010-2013, A. Hornung, M. Philips + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef OCTOMAP_SERVER_OCTOMAPSERVERMULTILAYER_H +#define OCTOMAP_SERVER_OCTOMAPSERVERMULTILAYER_H + +#include + +namespace octomap_server { +class OctomapServerMultilayer : public OctomapServer{ + +public: + OctomapServerMultilayer(ros::NodeHandle private_nh_ = ros::NodeHandle("~")); + virtual ~OctomapServerMultilayer(); + +protected: + struct ProjectedMap{ + double minZ; + double maxZ; + double z; // for visualization + std::string name; + nav_msgs::OccupancyGrid map; + }; + typedef std::vector MultilevelGrid; + + /// hook that is called after traversing all nodes + virtual void handlePreNodeTraversal(const ros::Time& rostime); + + /// updates the downprojected 2D map as either occupied or free + virtual void update2DMap(const OcTreeT::iterator& it, bool occupied); + + /// hook that is called after traversing all nodes + virtual void handlePostNodeTraversal(const ros::Time& rostime); + + std::vector m_multiMapPub; + ros::Subscriber m_attachedObjectsSub; + + std::vector m_armLinks; + std::vector m_armLinkOffsets; + + MultilevelGrid m_multiGridmap; + + +}; +} + +#endif + diff --git a/third_party/octomap_server/include/octomap_server/TrackingOctomapServer.h b/third_party/octomap_server/include/octomap_server/TrackingOctomapServer.h new file mode 100644 index 0000000..32daae3 --- /dev/null +++ b/third_party/octomap_server/include/octomap_server/TrackingOctomapServer.h @@ -0,0 +1,59 @@ +/* + * Copyright (c) 2012, D. Kuhner, P. Ruchti, University of Freiburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef OCTOMAP_SERVER_TRACKINGOCTOMAPSERVER_H_ +#define OCTOMAP_SERVER_TRACKINGOCTOMAPSERVER_H_ + +#include "octomap_server/OctomapServer.h" + +namespace octomap_server { + +class TrackingOctomapServer: public OctomapServer { +public: + TrackingOctomapServer(const std::string& filename = ""); + virtual ~TrackingOctomapServer(); + + void trackCallback(sensor_msgs::PointCloud2Ptr cloud); + void insertScan(const tf::Point& sensorOrigin, const PCLPointCloud& ground, const PCLPointCloud& nonground); + +protected: + void trackChanges(); + + bool listen_changes; + bool track_changes; + int min_change_pub; + std::string change_id_frame; + ros::Publisher pubFreeChangeSet; + ros::Publisher pubChangeSet; + ros::Subscriber subChangeSet; + ros::Subscriber subFreeChanges; +}; + +} /* namespace octomap */ +#endif /* TRACKINGOCTOMAPSERVER_H_ */ diff --git a/third_party/octomap_server/launch/octomap_mapping.launch b/third_party/octomap_server/launch/octomap_mapping.launch new file mode 100644 index 0000000..9a65d8e --- /dev/null +++ b/third_party/octomap_server/launch/octomap_mapping.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/third_party/octomap_server/launch/octomap_mapping_nodelet.launch b/third_party/octomap_server/launch/octomap_mapping_nodelet.launch new file mode 100644 index 0000000..0605c17 --- /dev/null +++ b/third_party/octomap_server/launch/octomap_mapping_nodelet.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/third_party/octomap_server/launch/octomap_tracking_client.launch b/third_party/octomap_server/launch/octomap_tracking_client.launch new file mode 100644 index 0000000..bae3e4d --- /dev/null +++ b/third_party/octomap_server/launch/octomap_tracking_client.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/third_party/octomap_server/launch/octomap_tracking_server.launch b/third_party/octomap_server/launch/octomap_tracking_server.launch new file mode 100644 index 0000000..55ea53c --- /dev/null +++ b/third_party/octomap_server/launch/octomap_tracking_server.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/third_party/octomap_server/mainpage.dox b/third_party/octomap_server/mainpage.dox new file mode 100644 index 0000000..2d8a6bc --- /dev/null +++ b/third_party/octomap_server/mainpage.dox @@ -0,0 +1,30 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b octomap_server is ... + + + + +\section codeapi Code API + + + + +*/ \ No newline at end of file diff --git a/third_party/octomap_server/nodelet_plugins.xml b/third_party/octomap_server/nodelet_plugins.xml new file mode 100644 index 0000000..783d76e --- /dev/null +++ b/third_party/octomap_server/nodelet_plugins.xml @@ -0,0 +1,8 @@ + + + + Nodelet for running the Octomap server + + + + diff --git a/third_party/octomap_server/package.xml b/third_party/octomap_server/package.xml new file mode 100644 index 0000000..9bc683f --- /dev/null +++ b/third_party/octomap_server/package.xml @@ -0,0 +1,53 @@ + + octomap_server + 0.6.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. + + Armin Hornung + Wolfgang Merkt + BSD + http://www.ros.org/wiki/octomap_server + https://github.com/OctoMap/octomap_mapping/issues + https://github.com/OctoMap/octomap_mapping + + + + + + + catkin + + roscpp + visualization_msgs + sensor_msgs + pcl_ros + pcl_conversions + nav_msgs + std_msgs + std_srvs + octomap + octomap_msgs + octomap_ros + dynamic_reconfigure + nodelet + libpcl-all-dev + + roscpp + visualization_msgs + sensor_msgs + pcl_ros + pcl_conversions + nav_msgs + std_msgs + std_srvs + octomap + octomap_msgs + octomap_ros + dynamic_reconfigure + nodelet + libpcl-all + + + + diff --git a/third_party/octomap_server/params/default.yaml b/third_party/octomap_server/params/default.yaml new file mode 100644 index 0000000..3834a2f --- /dev/null +++ b/third_party/octomap_server/params/default.yaml @@ -0,0 +1 @@ +# Empty file to load default parameters. diff --git a/third_party/octomap_server/scripts/octomap_eraser_cli.py b/third_party/octomap_server/scripts/octomap_eraser_cli.py new file mode 100755 index 0000000..407f282 --- /dev/null +++ b/third_party/octomap_server/scripts/octomap_eraser_cli.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python + +"""Clear a region specified by a global axis-aligned bounding box in stored +OctoMap. + +""" + +import sys +from time import sleep + +import roslib +roslib.load_manifest('octomap_server') +from geometry_msgs.msg import Point +import octomap_msgs.srv +import rospy + + +SRV_NAME = '/octomap_server/clear_bbx' +SRV_INTERFACE = octomap_msgs.srv.BoundingBoxQuery + + +if __name__ == '__main__': + min = Point(*[float(x) for x in sys.argv[1:4]]) + max = Point(*[float(x) for x in sys.argv[4:7]]) + + rospy.init_node('octomap_eraser_cli', anonymous=True) + sleep(1) + service = rospy.ServiceProxy(SRV_NAME, SRV_INTERFACE) + rospy.loginfo("Connected to %s service." % SRV_NAME) + + service(min, max) diff --git a/third_party/octomap_server/src/OctomapServer.cpp b/third_party/octomap_server/src/OctomapServer.cpp new file mode 100644 index 0000000..b67ad35 --- /dev/null +++ b/third_party/octomap_server/src/OctomapServer.cpp @@ -0,0 +1,1298 @@ +/* + * Copyright (c) 2010-2013, A. Hornung, University of Freiburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +using namespace octomap; +using octomap_msgs::Octomap; + +bool is_equal (double a, double b, double epsilon = 1.0e-7) +{ + return std::abs(a - b) < epsilon; +} + +namespace octomap_server{ + +OctomapServer::OctomapServer(const ros::NodeHandle private_nh_, const ros::NodeHandle &nh_,const std::string frame_id) +: m_nh(nh_), + m_nh_private(private_nh_), + m_pointCloudSub(NULL), + m_tfPointCloudSub(NULL), + m_reconfigureServer(m_config_mutex, private_nh_), + m_octree(NULL), + m_maxRange(-1.0), + m_worldFrameId(frame_id), m_baseFrameId("base_footprint"), + m_useHeightMap(true), + m_useColoredMap(false), + m_colorFactor(0.8), + m_latchedTopics(true), + m_publishFreeSpace(false), + m_res(0.1), + m_treeDepth(0), + m_maxTreeDepth(0), + m_pointcloudMinX(-std::numeric_limits::max()), + m_pointcloudMaxX(std::numeric_limits::max()), + m_pointcloudMinY(-std::numeric_limits::max()), + m_pointcloudMaxY(std::numeric_limits::max()), + m_pointcloudMinZ(-std::numeric_limits::max()), + m_pointcloudMaxZ(std::numeric_limits::max()), + m_occupancyMinZ(-std::numeric_limits::max()), + m_occupancyMaxZ(std::numeric_limits::max()), + m_minSizeX(0.0), m_minSizeY(0.0), + m_filterSpeckles(false), m_filterGroundPlane(false), + m_groundFilterDistance(0.04), m_groundFilterAngle(0.15), m_groundFilterPlaneDistance(0.07), + m_compressMap(true), + m_incrementalUpdate(false), + m_initConfig(true) +{ + double probHit, probMiss, thresMin, thresMax; + + m_nh_private.param("frame_id", m_worldFrameId, m_worldFrameId); + m_nh_private.param("base_frame_id", m_baseFrameId, m_baseFrameId); + m_nh_private.param("height_map", m_useHeightMap, m_useHeightMap); + m_nh_private.param("colored_map", m_useColoredMap, m_useColoredMap); + m_nh_private.param("color_factor", m_colorFactor, m_colorFactor); + + m_nh_private.param("pointcloud_min_x", m_pointcloudMinX,m_pointcloudMinX); + m_nh_private.param("pointcloud_max_x", m_pointcloudMaxX,m_pointcloudMaxX); + m_nh_private.param("pointcloud_min_y", m_pointcloudMinY,m_pointcloudMinY); + m_nh_private.param("pointcloud_max_y", m_pointcloudMaxY,m_pointcloudMaxY); + m_nh_private.param("pointcloud_min_z", m_pointcloudMinZ,m_pointcloudMinZ); + m_nh_private.param("pointcloud_max_z", m_pointcloudMaxZ,m_pointcloudMaxZ); + m_nh_private.param("occupancy_min_z", m_occupancyMinZ,m_occupancyMinZ); + m_nh_private.param("occupancy_max_z", m_occupancyMaxZ,m_occupancyMaxZ); + m_nh_private.param("min_x_size", m_minSizeX,m_minSizeX); + m_nh_private.param("min_y_size", m_minSizeY,m_minSizeY); + + m_nh_private.param("filter_speckles", m_filterSpeckles, m_filterSpeckles); + m_nh_private.param("filter_ground", m_filterGroundPlane, m_filterGroundPlane); + // distance of points from plane for RANSAC + m_nh_private.param("ground_filter/distance", m_groundFilterDistance, m_groundFilterDistance); + // angular derivation of found plane: + m_nh_private.param("ground_filter/angle", m_groundFilterAngle, m_groundFilterAngle); + // distance of found plane from z=0 to be detected as ground (e.g. to exclude tables) + m_nh_private.param("ground_filter/plane_distance", m_groundFilterPlaneDistance, m_groundFilterPlaneDistance); + + m_nh_private.param("sensor_model/max_range", m_maxRange, m_maxRange); + + m_nh_private.param("resolution", m_res, 0.1); + m_nh_private.param("sensor_model/hit", probHit, 0.7); + m_nh_private.param("sensor_model/miss", probMiss, 0.4); + m_nh_private.param("sensor_model/min", thresMin, 0.12); + m_nh_private.param("sensor_model/max", thresMax, 0.97); + m_nh_private.param("compress_map", m_compressMap, m_compressMap); + m_nh_private.param("incremental_2D_projection", m_incrementalUpdate, m_incrementalUpdate); + + if (m_filterGroundPlane && (m_pointcloudMinZ > 0.0 || m_pointcloudMaxZ < 0.0)){ + ROS_WARN_STREAM("You enabled ground filtering but incoming pointclouds will be pre-filtered in [" + <setProbHit(probHit); + m_octree->setProbMiss(probMiss); + m_octree->setClampingThresMin(thresMin); + m_octree->setClampingThresMax(thresMax); + m_treeDepth = m_octree->getTreeDepth(); + m_maxTreeDepth = m_treeDepth; + m_gridmap.info.resolution = m_res; + + double r, g, b, a; + m_nh_private.param("color/r", r, 0.0); + m_nh_private.param("color/g", g, 0.0); + m_nh_private.param("color/b", b, 1.0); + m_nh_private.param("color/a", a, 1.0); + m_color.r = r; + m_color.g = g; + m_color.b = b; + m_color.a = a; + + m_nh_private.param("color_free/r", r, 0.0); + m_nh_private.param("color_free/g", g, 1.0); + m_nh_private.param("color_free/b", b, 0.0); + m_nh_private.param("color_free/a", a, 1.0); + m_colorFree.r = r; + m_colorFree.g = g; + m_colorFree.b = b; + m_colorFree.a = a; + + m_nh_private.param("publish_free_space", m_publishFreeSpace, m_publishFreeSpace); + + m_nh_private.param("latch", m_latchedTopics, m_latchedTopics); + if (m_latchedTopics){ + ROS_INFO("Publishing latched (single publish will take longer, all topics are prepared)"); + } else + ROS_INFO("Publishing non-latched (topics are only prepared as needed, will only be re-published on map change"); + + m_markerPub = m_nh.advertise("occupied_cells_vis_array", 1, m_latchedTopics); + m_binaryMapPub = m_nh.advertise("octomap_binary", 1, m_latchedTopics); + m_fullMapPub = m_nh.advertise("octomap_full", 1, m_latchedTopics); + m_pointCloudPub = m_nh.advertise("octomap_point_cloud_centers", 1, m_latchedTopics); + m_mapPub = m_nh.advertise("projected_map", 5, m_latchedTopics); + m_fmarkerPub = m_nh.advertise("free_cells_vis_array", 1, m_latchedTopics); + + m_pointCloudSub = new message_filters::Subscriber (m_nh, "cloud_in", 5); + m_tfPointCloudSub = new tf::MessageFilter (*m_pointCloudSub, m_tfListener, m_worldFrameId, 5); + m_tfPointCloudSub->registerCallback(boost::bind(&OctomapServer::insertCloudCallback, this, _1)); + + m_octomapBinaryService = m_nh.advertiseService("octomap_binary", &OctomapServer::octomapBinarySrv, this); + m_octomapFullService = m_nh.advertiseService("octomap_full", &OctomapServer::octomapFullSrv, this); + m_clearBBXService = m_nh_private.advertiseService("clear_bbx", &OctomapServer::clearBBXSrv, this); + m_resetService = m_nh_private.advertiseService("reset", &OctomapServer::resetSrv, this); + + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&OctomapServer::reconfigureCallback, this, _1, _2); + m_reconfigureServer.setCallback(f); +} + +OctomapServer::~OctomapServer(){ + if (m_tfPointCloudSub){ + delete m_tfPointCloudSub; + m_tfPointCloudSub = NULL; + } + + if (m_pointCloudSub){ + delete m_pointCloudSub; + m_pointCloudSub = NULL; + } + + + if (m_octree){ + delete m_octree; + m_octree = NULL; + } + +} + +bool OctomapServer::openFile(const std::string& filename){ + if (filename.length() <= 3) + return false; + + std::string suffix = filename.substr(filename.length()-3, 3); + if (suffix== ".bt"){ + if (!m_octree->readBinary(filename)){ + return false; + } + } else if (suffix == ".ot"){ + AbstractOcTree* tree = AbstractOcTree::read(filename); + if (!tree){ + return false; + } + if (m_octree){ + delete m_octree; + m_octree = NULL; + } + m_octree = dynamic_cast(tree); + if (!m_octree){ + ROS_ERROR("Could not read OcTree in file, currently there are no other types supported in .ot"); + return false; + } + + } else{ + return false; + } + + ROS_INFO("Octomap file %s loaded (%zu nodes).", filename.c_str(),m_octree->size()); + + m_treeDepth = m_octree->getTreeDepth(); + m_maxTreeDepth = m_treeDepth; + m_res = m_octree->getResolution(); + m_gridmap.info.resolution = m_res; + double minX, minY, minZ; + double maxX, maxY, maxZ; + m_octree->getMetricMin(minX, minY, minZ); + m_octree->getMetricMax(maxX, maxY, maxZ); + + m_updateBBXMin[0] = m_octree->coordToKey(minX); + m_updateBBXMin[1] = m_octree->coordToKey(minY); + m_updateBBXMin[2] = m_octree->coordToKey(minZ); + + m_updateBBXMax[0] = m_octree->coordToKey(maxX); + m_updateBBXMax[1] = m_octree->coordToKey(maxY); + m_updateBBXMax[2] = m_octree->coordToKey(maxZ); + + publishAll(); + + return true; + +} + +void OctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){ + ros::WallTime startTime = ros::WallTime::now(); + + + // + // ground filtering in base frame + // + PCLPointCloud pc; // input cloud for filtering and ground-detection + pcl::fromROSMsg(*cloud, pc); + + tf::StampedTransform sensorToWorldTf; + try { + m_tfListener.lookupTransform(m_worldFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToWorldTf); + } catch(tf::TransformException& ex){ + ROS_ERROR_STREAM( "Transform error of sensor data: " << ex.what() << ", quitting callback"); + return; + } + + Eigen::Matrix4f sensorToWorld; + pcl_ros::transformAsMatrix(sensorToWorldTf, sensorToWorld); + + + // set up filter for height range, also removes NANs: + //* 定义pointcloud过滤器 + pcl::PassThrough pass_x; + pass_x.setFilterFieldName("x"); + pass_x.setFilterLimits(m_pointcloudMinX, m_pointcloudMaxX); + pcl::PassThrough pass_y; + pass_y.setFilterFieldName("y"); + pass_y.setFilterLimits(m_pointcloudMinY, m_pointcloudMaxY); + pcl::PassThrough pass_z; + pass_z.setFilterFieldName("z"); + pass_z.setFilterLimits(m_pointcloudMinZ, m_pointcloudMaxZ); + + PCLPointCloud pc_ground; // segmented ground plane + PCLPointCloud pc_nonground; // everything else + + if (m_filterGroundPlane){ + tf::StampedTransform sensorToBaseTf, baseToWorldTf; + try{ + m_tfListener.waitForTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, ros::Duration(0.2)); + m_tfListener.lookupTransform(m_baseFrameId, cloud->header.frame_id, cloud->header.stamp, sensorToBaseTf); + m_tfListener.lookupTransform(m_worldFrameId, m_baseFrameId, cloud->header.stamp, baseToWorldTf); + + + }catch(tf::TransformException& ex){ + ROS_ERROR_STREAM( "Transform error for ground plane filter: " << ex.what() << ", quitting callback.\n" + "You need to set the base_frame_id or disable filter_ground."); + } + + + Eigen::Matrix4f sensorToBase, baseToWorld; + pcl_ros::transformAsMatrix(sensorToBaseTf, sensorToBase); + pcl_ros::transformAsMatrix(baseToWorldTf, baseToWorld); + + // transform pointcloud from sensor frame to fixed robot frame + pcl::transformPointCloud(pc, pc, sensorToBase); + pass_x.setInputCloud(pc.makeShared()); + pass_x.filter(pc); + pass_y.setInputCloud(pc.makeShared()); + pass_y.filter(pc); + pass_z.setInputCloud(pc.makeShared()); + pass_z.filter(pc); + //* 这一步调用的是pcl库里面的算法，具体没有看，反正就得到了地面点和非地面点，然后填充了输入函数的2个容器 + filterGroundPlane(pc, pc_ground, pc_nonground); + + // transform clouds to world frame for insertion + pcl::transformPointCloud(pc_ground, pc_ground, baseToWorld); + pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld); + } else { + // directly transform to map frame: + pcl::transformPointCloud(pc, pc, sensorToWorld); + + // just filter height range: + pass_x.setInputCloud(pc.makeShared()); + pass_x.filter(pc); + pass_y.setInputCloud(pc.makeShared()); + pass_y.filter(pc); + pass_z.setInputCloud(pc.makeShared()); + pass_z.filter(pc); + + pc_nonground = pc; + // pc_nonground is empty without ground segmentation + pc_ground.header = pc.header; + pc_nonground.header = pc.header; + } + + + insertScan(sensorToWorldTf.getOrigin(), pc_ground, pc_nonground); + + double total_elapsed = (ros::WallTime::now() - startTime).toSec(); + ROS_DEBUG("Pointcloud insertion in OctomapServer done (%zu+%zu pts (ground/nonground), %f sec)", pc_ground.size(), pc_nonground.size(), total_elapsed); + + publishAll(cloud->header.stamp); +} + +void OctomapServer::insertScan(const tf::Point& sensorOriginTf, const PCLPointCloud& ground, const PCLPointCloud& nonground){ + point3d sensorOrigin = pointTfToOctomap(sensorOriginTf); + + if (!m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMin) + || !m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMax)) + { + ROS_ERROR_STREAM("Could not generate Key for origin "< 0){ + for (PCLPointCloud::const_iterator it = ground.begin(); it != ground.end(); ++it){ + point3d point(it->x, it->y, it->z); + // maxrange check 如果point距离sensor超过最大传感器范围，则使point距离等于传感器最大范围 + if ((m_maxRange > 0.0) && ((point - sensorOrigin).norm() > m_maxRange) ) { + point = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange; + } + + // only clear space (ground points) + if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){ + free_cells.insert(m_keyRay.begin(), m_keyRay.end()); + } + + octomap::OcTreeKey endKey; + if (m_octree->coordToKeyChecked(point, endKey)){ + updateMinKey(endKey, m_updateBBXMin); //* xyz3个坐标中，如果当前点更小，则用更小的更新 + updateMaxKey(endKey, m_updateBBXMax); + } else{ + ROS_ERROR_STREAM("Could not generate Key for endpoint "<x, it->y, it->z); + if(point(2) < 0.0) continue; + // maxrange check + if ((m_maxRange < 0.0) || ((point - sensorOrigin).norm() <= m_maxRange) ) { + + // free cells + if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)){ + free_cells.insert(m_keyRay.begin(), m_keyRay.end()); + } + // occupied endpoint + OcTreeKey key; + if (m_octree->coordToKeyChecked(point, key)){ + occupied_cells.insert(key); + + updateMinKey(key, m_updateBBXMin); + updateMaxKey(key, m_updateBBXMax); + +#ifdef COLOR_OCTOMAP_SERVER // NB: Only read and interpret color if it's an occupied node + // m_octree->averageNodeColor(it->x, it->y, it->z, /*r=*/it->r, /*g=*/it->g, /*b=*/it->b); + //! change: give all the occupied grid color + m_octree->averageNodeColor(it->x, it->y, it->z, /*r=*/0, /*g=*/0, /*b=*/255); + +#endif + } + } else {// ray longer than maxrange:; + point3d new_end = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange; + if (m_octree->computeRayKeys(sensorOrigin, new_end, m_keyRay)){ + free_cells.insert(m_keyRay.begin(), m_keyRay.end()); + + octomap::OcTreeKey endKey; + if (m_octree->coordToKeyChecked(new_end, endKey)){ + free_cells.insert(endKey); + updateMinKey(endKey, m_updateBBXMin); + updateMaxKey(endKey, m_updateBBXMax); + } else{ + ROS_ERROR_STREAM("Could not generate Key for endpoint "<updateNode(*it, false); + } + } + + // now mark all occupied cells: + for (KeySet::iterator it = occupied_cells.begin(), end=occupied_cells.end(); it!= end; it++) { + m_octree->updateNode(*it, true); + } + + // TODO: eval lazy+updateInner vs. proper insertion + // non-lazy by default (updateInnerOccupancy() too slow for large maps) + //m_octree->updateInnerOccupancy(); + octomap::point3d minPt, maxPt; + ROS_DEBUG_STREAM("Bounding box keys (before): " << m_updateBBXMin[0] << " " <getNodeSize( m_maxTreeDepth ) - 1; +// tmpMax[1]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1; +// tmpMax[2]+= m_octree->getNodeSize( m_maxTreeDepth ) - 1; +// m_updateBBXMin = tmpMin; +// m_updateBBXMax = tmpMax; +// } + + // TODO: we could also limit the bbx to be within the map bounds here (see publishing check) + minPt = m_octree->keyToCoord(m_updateBBXMin); + maxPt = m_octree->keyToCoord(m_updateBBXMax); + ROS_DEBUG_STREAM("Updated area bounding box: "<< minPt << " - "<prune(); + +#ifdef COLOR_OCTOMAP_SERVER + if (colors) + { + delete[] colors; + colors = NULL; + } +#endif +} + + + + + + +void OctomapServer::publishAll(const ros::Time& rostime){ + ros::WallTime startTime = ros::WallTime::now(); + size_t octomapSize = m_octree->size(); + // TODO: estimate num occ. voxels for size of arrays (reserve) + if (octomapSize <= 1){ + ROS_WARN("Nothing to publish, octree is empty"); + return; + } + + bool publishFreeMarkerArray = m_publishFreeSpace && (m_latchedTopics || m_fmarkerPub.getNumSubscribers() > 0); + bool publishMarkerArray = (m_latchedTopics || m_markerPub.getNumSubscribers() > 0); + bool publishPointCloud = (m_latchedTopics || m_pointCloudPub.getNumSubscribers() > 0); + bool publishBinaryMap = (m_latchedTopics || m_binaryMapPub.getNumSubscribers() > 0); + bool publishFullMap = (m_latchedTopics || m_fullMapPub.getNumSubscribers() > 0); + m_publish2DMap = (m_latchedTopics || m_mapPub.getNumSubscribers() > 0); + + //! 这里的 Free & Occupied MarkerArray 都是数组的形式，数组中的每一个元素都是一个marker verctor，存储当前deepth对应的voxel的信息 + // init markers for free space: + visualization_msgs::MarkerArray freeNodesVis; + // each array stores all cubes of a different size, one for each depth level: + freeNodesVis.markers.resize(m_treeDepth+1); + + geometry_msgs::Pose pose; + pose.orientation = tf::createQuaternionMsgFromYaw(0.0); + + // init markers: + visualization_msgs::MarkerArray occupiedNodesVis; + // each array stores all cubes of a different size, one for each depth level: + occupiedNodesVis.markers.resize(m_treeDepth+1); + + // init pointcloud: + pcl::PointCloud pclCloud; + + // call pre-traversal hook: + handlePreNodeTraversal(rostime); //* 和2D地图有关的预处理 + + // now, traverse all leafs in the tree: + for (OcTreeT::iterator it = m_octree->begin(m_maxTreeDepth), + end = m_octree->end(); it != end; ++it) + { + bool inUpdateBBX = isInUpdateBBX(it); //* 从iter变到key，从key获得lower corner坐标 + + // call general hook: + handleNode(it); //* do nothing + if (inUpdateBBX) + handleNodeInBBX(it); //* do nothing + + if (m_octree->isNodeOccupied(*it)){ + double z = it.getZ(); + double half_size = it.getSize() / 2.0; + if (z + half_size > m_occupancyMinZ && z - half_size < m_occupancyMaxZ) + { + double size = it.getSize(); + double x = it.getX(); + double y = it.getY(); +#ifdef COLOR_OCTOMAP_SERVER + int r = it->getColor().r; + int g = it->getColor().g; + int b = it->getColor().b; +#endif + + // Ignore speckles in the map: + if (m_filterSpeckles && (it.getDepth() == m_treeDepth +1) && isSpeckleNode(it.getKey())){ //! 这个isSpeckleNode函数里面包含了怎么遍历邻居！ + ROS_DEBUG("Ignoring single speckle at (%f,%f,%f)", x, y, z); + continue; + } // else: current octree node is no speckle, send it out + + handleOccupiedNode(it); //* 函数里面是对2D地图的更新 + if (inUpdateBBX) + handleOccupiedNodeInBBX(it); //? 函数里面还是是对2D地图的更新？？ + + + //create marker: + if (publishMarkerArray){ + unsigned idx = it.getDepth(); + assert(idx < occupiedNodesVis.markers.size()); // 如果它的条件返回错误,则终止程序执行 + + geometry_msgs::Point cubeCenter; + cubeCenter.x = x; + cubeCenter.y = y; + cubeCenter.z = z; + + occupiedNodesVis.markers[idx].points.push_back(cubeCenter); + if (m_useHeightMap){ + double minX, minY, minZ, maxX, maxY, maxZ; + m_octree->getMetricMin(minX, minY, minZ); + m_octree->getMetricMax(maxX, maxY, maxZ); + + double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor; + occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h)); + } + +#ifdef COLOR_OCTOMAP_SERVER + if (m_useColoredMap) { + 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? + occupiedNodesVis.markers[idx].colors.push_back(_color); + // std::cout<<"color:"<<_color< mark as free in 2D map if unknown so far + double z = it.getZ(); + double half_size = it.getSize() / 2.0; + if (z + half_size > m_occupancyMinZ && z - half_size < m_occupancyMaxZ) + { + handleFreeNode(it); //* 同上，也是两个里面是对2D地图的更新的函数 + if (inUpdateBBX) + handleFreeNodeInBBX(it); + + if (m_publishFreeSpace){ + double x = it.getX(); + double y = it.getY(); + + //create marker for free space: + if (publishFreeMarkerArray){ + unsigned idx = it.getDepth(); + assert(idx < freeNodesVis.markers.size()); + + geometry_msgs::Point cubeCenter; + cubeCenter.x = x; + cubeCenter.y = y; + cubeCenter.z = z; + + freeNodesVis.markers[idx].points.push_back(cubeCenter); + } + } + + } + } + } + + // call post-traversal hook: + handlePostNodeTraversal(rostime); //* Pub 2D 地图 + + // finish MarkerArray: + if (publishMarkerArray){ + for (unsigned i= 0; i < occupiedNodesVis.markers.size(); ++i){ + double size = m_octree->getNodeSize(i); + + occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId; + occupiedNodesVis.markers[i].header.stamp = rostime; + occupiedNodesVis.markers[i].ns = "world"; + occupiedNodesVis.markers[i].id = i; + occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST; + occupiedNodesVis.markers[i].scale.x = size; + occupiedNodesVis.markers[i].scale.y = size; + occupiedNodesVis.markers[i].scale.z = size; + if (!m_useColoredMap) + occupiedNodesVis.markers[i].color = m_color; + occupiedNodesVis.markers[i].color.a = 0.6; + + + if (occupiedNodesVis.markers[i].points.size() > 0) + occupiedNodesVis.markers[i].action = visualization_msgs::Marker::ADD; + else + occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE; + } + + m_markerPub.publish(occupiedNodesVis); + } + + + // finish FreeMarkerArray: + if (publishFreeMarkerArray){ + for (unsigned i= 0; i < freeNodesVis.markers.size(); ++i){ + double size = m_octree->getNodeSize(i); + + freeNodesVis.markers[i].header.frame_id = m_worldFrameId; + freeNodesVis.markers[i].header.stamp = rostime; + freeNodesVis.markers[i].ns = "world"; + freeNodesVis.markers[i].id = i; + freeNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST; + freeNodesVis.markers[i].scale.x = size; + freeNodesVis.markers[i].scale.y = size; + freeNodesVis.markers[i].scale.z = size; + freeNodesVis.markers[i].color = m_colorFree; + freeNodesVis.markers[i].color.a = 0.6; + + + if (freeNodesVis.markers[i].points.size() > 0) + freeNodesVis.markers[i].action = visualization_msgs::Marker::ADD; + else + freeNodesVis.markers[i].action = visualization_msgs::Marker::DELETE; + } + + m_fmarkerPub.publish(freeNodesVis); + } + + + // finish pointcloud: + if (publishPointCloud){ + sensor_msgs::PointCloud2 cloud; + pcl::toROSMsg (pclCloud, cloud); + cloud.header.frame_id = m_worldFrameId; + cloud.header.stamp = rostime; + m_pointCloudPub.publish(cloud); + } + + if (publishBinaryMap) + publishBinaryOctoMap(rostime); + + if (publishFullMap) + publishFullOctoMap(rostime); + + + double total_elapsed = (ros::WallTime::now() - startTime).toSec(); + ROS_DEBUG("Map publishing in OctomapServer took %f sec", total_elapsed); + +} + + +bool OctomapServer::octomapBinarySrv(OctomapSrv::Request &req, + OctomapSrv::Response &res) +{ + ros::WallTime startTime = ros::WallTime::now(); + ROS_INFO("Sending binary map data on service request"); + res.map.header.frame_id = m_worldFrameId; + res.map.header.stamp = ros::Time::now(); + if (!octomap_msgs::binaryMapToMsg(*m_octree, res.map)) + return false; + + double total_elapsed = (ros::WallTime::now() - startTime).toSec(); + ROS_INFO("Binary octomap sent in %f sec", total_elapsed); + return true; +} + +bool OctomapServer::octomapFullSrv(OctomapSrv::Request &req, + OctomapSrv::Response &res) +{ + ROS_INFO("Sending full map data on service request"); + res.map.header.frame_id = m_worldFrameId; + res.map.header.stamp = ros::Time::now(); + + + if (!octomap_msgs::fullMapToMsg(*m_octree, res.map)) + return false; + + return true; +} + +bool OctomapServer::clearBBXSrv(BBXSrv::Request& req, BBXSrv::Response& resp){ + point3d min = pointMsgToOctomap(req.min); + point3d max = pointMsgToOctomap(req.max); + + double thresMin = m_octree->getClampingThresMin(); + for(OcTreeT::leaf_bbx_iterator it = m_octree->begin_leafs_bbx(min,max), + end=m_octree->end_leafs_bbx(); it!= end; ++it){ + + it->setLogOdds(octomap::logodds(thresMin)); + // m_octree->updateNode(it.getKey(), -6.0f); + } + // TODO: eval which is faster (setLogOdds+updateInner or updateNode) + m_octree->updateInnerOccupancy(); + + publishAll(ros::Time::now()); + + return true; +} + +bool OctomapServer::resetSrv(std_srvs::Empty::Request& req, std_srvs::Empty::Response& resp) { + visualization_msgs::MarkerArray occupiedNodesVis; + occupiedNodesVis.markers.resize(m_treeDepth +1); + ros::Time rostime = ros::Time::now(); + m_octree->clear(); + // clear 2D map: + m_gridmap.data.clear(); + m_gridmap.info.height = 0.0; + m_gridmap.info.width = 0.0; + m_gridmap.info.resolution = 0.0; + m_gridmap.info.origin.position.x = 0.0; + m_gridmap.info.origin.position.y = 0.0; + + ROS_INFO("Cleared octomap"); + publishAll(rostime); + + publishBinaryOctoMap(rostime); + for (unsigned i= 0; i < occupiedNodesVis.markers.size(); ++i){ + + occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId; + occupiedNodesVis.markers[i].header.stamp = rostime; + occupiedNodesVis.markers[i].ns = "map"; + occupiedNodesVis.markers[i].id = i; + occupiedNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST; + occupiedNodesVis.markers[i].action = visualization_msgs::Marker::DELETE; + } + + m_markerPub.publish(occupiedNodesVis); + + visualization_msgs::MarkerArray freeNodesVis; + freeNodesVis.markers.resize(m_treeDepth +1); + + for (unsigned i= 0; i < freeNodesVis.markers.size(); ++i){ + + freeNodesVis.markers[i].header.frame_id = m_worldFrameId; + freeNodesVis.markers[i].header.stamp = rostime; + freeNodesVis.markers[i].ns = "map"; + freeNodesVis.markers[i].id = i; + freeNodesVis.markers[i].type = visualization_msgs::Marker::CUBE_LIST; + freeNodesVis.markers[i].action = visualization_msgs::Marker::DELETE; + } + m_fmarkerPub.publish(freeNodesVis); + + return true; +} + +void OctomapServer::publishBinaryOctoMap(const ros::Time& rostime) const{ + + Octomap map; + map.header.frame_id = m_worldFrameId; + map.header.stamp = rostime; + + if (octomap_msgs::binaryMapToMsg(*m_octree, map)) + m_binaryMapPub.publish(map); + else + ROS_ERROR("Error serializing OctoMap"); +} + +void OctomapServer::publishFullOctoMap(const ros::Time& rostime) const{ + + Octomap map; + map.header.frame_id = m_worldFrameId; + map.header.stamp = rostime; + + if (octomap_msgs::fullMapToMsg(*m_octree, map)) + m_fullMapPub.publish(map); + else + ROS_ERROR("Error serializing OctoMap"); + +} + + +void OctomapServer::filterGroundPlane(const PCLPointCloud& pc, PCLPointCloud& ground, PCLPointCloud& nonground) const{ + ground.header = pc.header; + nonground.header = pc.header; + + if (pc.size() < 50){ + ROS_WARN("Pointcloud in OctomapServer too small, skipping ground plane extraction"); + nonground = pc; + } else { + // plane detection for ground plane removal: + pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers (new pcl::PointIndices); + + // Create the segmentation object and set up: + pcl::SACSegmentation seg; + seg.setOptimizeCoefficients (true); + // TODO: maybe a filtering based on the surface normals might be more robust / accurate? + seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setMaxIterations(200); + seg.setDistanceThreshold (m_groundFilterDistance); + seg.setAxis(Eigen::Vector3f(0,0,1)); + seg.setEpsAngle(m_groundFilterAngle); + + + PCLPointCloud cloud_filtered(pc); + // Create the filtering object + pcl::ExtractIndices extract; + bool groundPlaneFound = false; + + while(cloud_filtered.size() > 10 && !groundPlaneFound){ + seg.setInputCloud(cloud_filtered.makeShared()); + seg.segment (*inliers, *coefficients); + if (inliers->indices.size () == 0){ + ROS_INFO("PCL segmentation did not find any plane."); + + break; + } + + extract.setInputCloud(cloud_filtered.makeShared()); + extract.setIndices(inliers); + + if (std::abs(coefficients->values.at(3)) < m_groundFilterPlaneDistance){ + ROS_DEBUG("Ground plane found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(), + coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3)); + extract.setNegative (false); + extract.filter (ground); + + // remove ground points from full pointcloud: + // workaround for PCL bug: + if(inliers->indices.size() != cloud_filtered.size()){ + extract.setNegative(true); + PCLPointCloud cloud_out; + extract.filter(cloud_out); + nonground += cloud_out; + cloud_filtered = cloud_out; + } + + groundPlaneFound = true; + } else{ + ROS_DEBUG("Horizontal plane (not ground) found: %zu/%zu inliers. Coeff: %f %f %f %f", inliers->indices.size(), cloud_filtered.size(), + coefficients->values.at(0), coefficients->values.at(1), coefficients->values.at(2), coefficients->values.at(3)); + pcl::PointCloud cloud_out; + extract.setNegative (false); + extract.filter(cloud_out); + nonground +=cloud_out; + // debug + // pcl::PCDWriter writer; + // writer.write("nonground_plane.pcd",cloud_out, false); + + // remove current plane from scan for next iteration: + // workaround for PCL bug: + if(inliers->indices.size() != cloud_filtered.size()){ + extract.setNegative(true); + cloud_out.points.clear(); + extract.filter(cloud_out); + cloud_filtered = cloud_out; + } else{ + cloud_filtered.points.clear(); + } + } + + } + // TODO: also do this if overall starting pointcloud too small? + if (!groundPlaneFound){ // no plane found or remaining points too small + ROS_WARN("No ground plane found in scan"); + + // do a rough fitlering on height to prevent spurious obstacles + pcl::PassThrough second_pass; + second_pass.setFilterFieldName("z"); + second_pass.setFilterLimits(-m_groundFilterPlaneDistance, m_groundFilterPlaneDistance); + second_pass.setInputCloud(pc.makeShared()); + second_pass.filter(ground); + + second_pass.setFilterLimitsNegative (true); + second_pass.filter(nonground); + } + + // debug: + // pcl::PCDWriter writer; + // if (pc_ground.size() > 0) + // writer.write("ground.pcd",pc_ground, false); + // if (pc_nonground.size() > 0) + // writer.write("nonground.pcd",pc_nonground, false); + + } + + +} + +void OctomapServer::handlePreNodeTraversal(const ros::Time& rostime){ + if (m_publish2DMap){ + // init projected 2D map: + m_gridmap.header.frame_id = m_worldFrameId; + m_gridmap.header.stamp = rostime; + nav_msgs::MapMetaData oldMapInfo = m_gridmap.info; + + // TODO: move most of this stuff into c'tor and init map only once (adjust if size changes) + double minX, minY, minZ, maxX, maxY, maxZ; + m_octree->getMetricMin(minX, minY, minZ); + m_octree->getMetricMax(maxX, maxY, maxZ); + + octomap::point3d minPt(minX, minY, minZ); + octomap::point3d maxPt(maxX, maxY, maxZ); + octomap::OcTreeKey minKey = m_octree->coordToKey(minPt, m_maxTreeDepth); + octomap::OcTreeKey maxKey = m_octree->coordToKey(maxPt, m_maxTreeDepth); + + ROS_DEBUG("MinKey: %d %d %d / MaxKey: %d %d %d", minKey[0], minKey[1], minKey[2], maxKey[0], maxKey[1], maxKey[2]); + + // add padding if requested (= new min/maxPts in x&y): + double halfPaddedX = 0.5*m_minSizeX; + double halfPaddedY = 0.5*m_minSizeY; + minX = std::min(minX, -halfPaddedX); + maxX = std::max(maxX, halfPaddedX); + minY = std::min(minY, -halfPaddedY); + maxY = std::max(maxY, halfPaddedY); + minPt = octomap::point3d(minX, minY, minZ); + maxPt = octomap::point3d(maxX, maxY, maxZ); + + OcTreeKey paddedMaxKey; + if (!m_octree->coordToKeyChecked(minPt, m_maxTreeDepth, m_paddedMinKey)){ + ROS_ERROR("Could not create padded min OcTree key at %f %f %f", minPt.x(), minPt.y(), minPt.z()); + return; + } + if (!m_octree->coordToKeyChecked(maxPt, m_maxTreeDepth, paddedMaxKey)){ + ROS_ERROR("Could not create padded max OcTree key at %f %f %f", maxPt.x(), maxPt.y(), maxPt.z()); + return; + } + + 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]); + assert(paddedMaxKey[0] >= maxKey[0] && paddedMaxKey[1] >= maxKey[1]); + + m_multires2DScale = 1 << (m_treeDepth - m_maxTreeDepth); + m_gridmap.info.width = (paddedMaxKey[0] - m_paddedMinKey[0])/m_multires2DScale +1; + m_gridmap.info.height = (paddedMaxKey[1] - m_paddedMinKey[1])/m_multires2DScale +1; + + int mapOriginX = minKey[0] - m_paddedMinKey[0]; + int mapOriginY = minKey[1] - m_paddedMinKey[1]; + assert(mapOriginX >= 0 && mapOriginY >= 0); + + // might not exactly be min / max of octree: + octomap::point3d origin = m_octree->keyToCoord(m_paddedMinKey, m_treeDepth); + double gridRes = m_octree->getNodeSize(m_maxTreeDepth); + m_projectCompleteMap = (!m_incrementalUpdate || (std::abs(gridRes-m_gridmap.info.resolution) > 1e-6)); + m_gridmap.info.resolution = gridRes; + m_gridmap.info.origin.position.x = origin.x() - gridRes*0.5; + m_gridmap.info.origin.position.y = origin.y() - gridRes*0.5; + if (m_maxTreeDepth != m_treeDepth){ + m_gridmap.info.origin.position.x -= m_res/2.0; + m_gridmap.info.origin.position.y -= m_res/2.0; + } + + // workaround for multires. projection not working properly for inner nodes: + // force re-building complete map + if (m_maxTreeDepth < m_treeDepth) + m_projectCompleteMap = true; + + + if(m_projectCompleteMap){ + ROS_DEBUG("Rebuilding complete 2D map"); + m_gridmap.data.clear(); + // init to unknown: + m_gridmap.data.resize(m_gridmap.info.width * m_gridmap.info.height, -1); + + } else { + + if (mapChanged(oldMapInfo, m_gridmap.info)){ + ROS_DEBUG("2D grid map size changed to %dx%d", m_gridmap.info.width, m_gridmap.info.height); + adjustMapData(m_gridmap, oldMapInfo); + } + nav_msgs::OccupancyGrid::_data_type::iterator startIt; + size_t mapUpdateBBXMinX = std::max(0, (int(m_updateBBXMin[0]) - int(m_paddedMinKey[0]))/int(m_multires2DScale)); + size_t mapUpdateBBXMinY = std::max(0, (int(m_updateBBXMin[1]) - int(m_paddedMinKey[1]))/int(m_multires2DScale)); + size_t mapUpdateBBXMaxX = std::min(int(m_gridmap.info.width-1), (int(m_updateBBXMax[0]) - int(m_paddedMinKey[0]))/int(m_multires2DScale)); + size_t mapUpdateBBXMaxY = std::min(int(m_gridmap.info.height-1), (int(m_updateBBXMax[1]) - int(m_paddedMinKey[1]))/int(m_multires2DScale)); + + assert(mapUpdateBBXMaxX > mapUpdateBBXMinX); + assert(mapUpdateBBXMaxY > mapUpdateBBXMinY); + + size_t numCols = mapUpdateBBXMaxX-mapUpdateBBXMinX +1; + + // test for max idx: + uint max_idx = m_gridmap.info.width*mapUpdateBBXMaxY + mapUpdateBBXMaxX; + if (max_idx >= m_gridmap.data.size()) + 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); + + // reset proj. 2D map in bounding box: + for (unsigned int j = mapUpdateBBXMinY; j <= mapUpdateBBXMaxY; ++j){ + std::fill_n(m_gridmap.data.begin() + m_gridmap.info.width*j+mapUpdateBBXMinX, + numCols, -1); + } + + } + + + + } + +} + +void OctomapServer::handlePostNodeTraversal(const ros::Time& rostime){ + + if (m_publish2DMap) + m_mapPub.publish(m_gridmap); +} + +void OctomapServer::handleOccupiedNode(const OcTreeT::iterator& it){ + + if (m_publish2DMap && m_projectCompleteMap){ + update2DMap(it, true); + } +} + +void OctomapServer::handleFreeNode(const OcTreeT::iterator& it){ + + if (m_publish2DMap && m_projectCompleteMap){ + update2DMap(it, false); + } +} + +void OctomapServer::handleOccupiedNodeInBBX(const OcTreeT::iterator& it){ + + if (m_publish2DMap && !m_projectCompleteMap){ + update2DMap(it, true); + } +} + +void OctomapServer::handleFreeNodeInBBX(const OcTreeT::iterator& it){ + + if (m_publish2DMap && !m_projectCompleteMap){ + update2DMap(it, false); + } +} + +void OctomapServer::update2DMap(const OcTreeT::iterator& it, bool occupied){ + + // update 2D map (occupied always overrides): + + if (it.getDepth() == m_maxTreeDepth){ + unsigned idx = mapIdx(it.getKey()); + if (occupied) + m_gridmap.data[mapIdx(it.getKey())] = 100; + else if (m_gridmap.data[idx] == -1){ + m_gridmap.data[idx] = 0; + } + + } else{ + int intSize = 1 << (m_maxTreeDepth - it.getDepth()); + octomap::OcTreeKey minKey=it.getIndexKey(); + for(int dx=0; dx < intSize; dx++){ + int i = (minKey[0]+dx - m_paddedMinKey[0])/m_multires2DScale; + for(int dy=0; dy < intSize; dy++){ + unsigned idx = mapIdx(i, (minKey[1]+dy - m_paddedMinKey[1])/m_multires2DScale); + if (occupied) + m_gridmap.data[idx] = 100; + else if (m_gridmap.data[idx] == -1){ + m_gridmap.data[idx] = 0; + } + } + } + } + + +} + + + +bool OctomapServer::isSpeckleNode(const OcTreeKey&nKey) const { + OcTreeKey key; + bool neighborFound = false; + for (key[2] = nKey[2] - 1; !neighborFound && key[2] <= nKey[2] + 1; ++key[2]){ + for (key[1] = nKey[1] - 1; !neighborFound && key[1] <= nKey[1] + 1; ++key[1]){ + for (key[0] = nKey[0] - 1; !neighborFound && key[0] <= nKey[0] + 1; ++key[0]){ + if (key != nKey){ + OcTreeNode* node = m_octree->search(key); + if (node && m_octree->isNodeOccupied(node)){ + // we have a neighbor => break! + neighborFound = true; + } + } + } + } + } + + return neighborFound; +} + +void OctomapServer::reconfigureCallback(octomap_server::OctomapServerConfig& config, uint32_t level){ + if (m_maxTreeDepth != unsigned(config.max_depth)) + m_maxTreeDepth = unsigned(config.max_depth); + else{ + m_pointcloudMinZ = config.pointcloud_min_z; + m_pointcloudMaxZ = config.pointcloud_max_z; + m_occupancyMinZ = config.occupancy_min_z; + m_occupancyMaxZ = config.occupancy_max_z; + m_filterSpeckles = config.filter_speckles; + m_filterGroundPlane = config.filter_ground; + m_compressMap = config.compress_map; + m_incrementalUpdate = config.incremental_2D_projection; + + // Parameters with a namespace require an special treatment at the beginning, as dynamic reconfigure + // will overwrite them because the server is not able to match parameters' names. + if (m_initConfig){ + // If parameters do not have the default value, dynamic reconfigure server should be updated. + if(!is_equal(m_groundFilterDistance, 0.04)) + config.ground_filter_distance = m_groundFilterDistance; + if(!is_equal(m_groundFilterAngle, 0.15)) + config.ground_filter_angle = m_groundFilterAngle; + if(!is_equal( m_groundFilterPlaneDistance, 0.07)) + config.ground_filter_plane_distance = m_groundFilterPlaneDistance; + if(!is_equal(m_maxRange, -1.0)) + config.sensor_model_max_range = m_maxRange; + if(!is_equal(m_octree->getProbHit(), 0.7)) + config.sensor_model_hit = m_octree->getProbHit(); + if(!is_equal(m_octree->getProbMiss(), 0.4)) + config.sensor_model_miss = m_octree->getProbMiss(); + if(!is_equal(m_octree->getClampingThresMin(), 0.12)) + config.sensor_model_min = m_octree->getClampingThresMin(); + if(!is_equal(m_octree->getClampingThresMax(), 0.97)) + config.sensor_model_max = m_octree->getClampingThresMax(); + m_initConfig = false; + + boost::recursive_mutex::scoped_lock reconf_lock(m_config_mutex); + m_reconfigureServer.updateConfig(config); + } + else{ + m_groundFilterDistance = config.ground_filter_distance; + m_groundFilterAngle = config.ground_filter_angle; + m_groundFilterPlaneDistance = config.ground_filter_plane_distance; + m_maxRange = config.sensor_model_max_range; + m_octree->setClampingThresMin(config.sensor_model_min); + m_octree->setClampingThresMax(config.sensor_model_max); + + // Checking values that might create unexpected behaviors. + if (is_equal(config.sensor_model_hit, 1.0)) + config.sensor_model_hit -= 1.0e-6; + m_octree->setProbHit(config.sensor_model_hit); + if (is_equal(config.sensor_model_miss, 0.0)) + config.sensor_model_miss += 1.0e-6; + m_octree->setProbMiss(config.sensor_model_miss); + } + } + publishAll(); +} + +void OctomapServer::adjustMapData(nav_msgs::OccupancyGrid& map, const nav_msgs::MapMetaData& oldMapInfo) const{ + if (map.info.resolution != oldMapInfo.resolution){ + ROS_ERROR("Resolution of map changed, cannot be adjusted"); + return; + } + + int i_off = int((oldMapInfo.origin.position.x - map.info.origin.position.x)/map.info.resolution +0.5); + int j_off = int((oldMapInfo.origin.position.y - map.info.origin.position.y)/map.info.resolution +0.5); + + if (i_off < 0 || j_off < 0 + || oldMapInfo.width + i_off > map.info.width + || oldMapInfo.height + j_off > map.info.height) + { + ROS_ERROR("New 2D map does not contain old map area, this case is not implemented"); + return; + } + + nav_msgs::OccupancyGrid::_data_type oldMapData = map.data; + + map.data.clear(); + // init to unknown: + map.data.resize(map.info.width * map.info.height, -1); + + nav_msgs::OccupancyGrid::_data_type::iterator fromStart, fromEnd, toStart; + + for (int j =0; j < int(oldMapInfo.height); ++j ){ + // copy chunks, row by row: + fromStart = oldMapData.begin() + j*oldMapInfo.width; + fromEnd = fromStart + oldMapInfo.width; + toStart = map.data.begin() + ((j+j_off)*m_gridmap.info.width + i_off); + copy(fromStart, fromEnd, toStart); + +// for (int i =0; i < int(oldMapInfo.width); ++i){ +// map.data[m_gridmap.info.width*(j+j_off) +i+i_off] = oldMapData[oldMapInfo.width*j +i]; +// } + + } + +} + + +std_msgs::ColorRGBA OctomapServer::heightMapColor(double h) { + + std_msgs::ColorRGBA color; + color.a = 1.0; + // blend over HSV-values (more colors) + + double s = 1.0; + double v = 1.0; + + h -= floor(h); + h *= 6; + int i; + double m, n, f; + + i = floor(h); + f = h - i; + if (!(i & 1)) + f = 1 - f; // if i is even + m = v * (1 - s); + n = v * (1 - s * f); + + switch (i) { + case 6: + case 0: + color.r = v; color.g = n; color.b = m; + break; + case 1: + color.r = n; color.g = v; color.b = m; + break; + case 2: + color.r = m; color.g = v; color.b = n; + break; + case 3: + color.r = m; color.g = n; color.b = v; + break; + case 4: + color.r = n; color.g = m; color.b = v; + break; + case 5: + color.r = v; color.g = m; color.b = n; + break; + default: + color.r = 1; color.g = 0.5; color.b = 0.5; + break; + } + + return color; +} +} + + + diff --git a/third_party/octomap_server/src/OctomapServerMultilayer.cpp b/third_party/octomap_server/src/OctomapServerMultilayer.cpp new file mode 100644 index 0000000..8b5c822 --- /dev/null +++ b/third_party/octomap_server/src/OctomapServerMultilayer.cpp @@ -0,0 +1,268 @@ +/* + * Copyright (c) 2011-2013, A. Hornung, M. Philips + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +using namespace octomap; + +namespace octomap_server{ + + + +OctomapServerMultilayer::OctomapServerMultilayer(ros::NodeHandle private_nh_) +: OctomapServer(private_nh_) +{ + + // TODO: callback for arm_navigation attached objects was removed, is + // there a replacement functionality? + + // TODO: param maps, limits + // right now 0: base, 1: spine, 2: arms + ProjectedMap m; + m.name = "projected_base_map"; + m.minZ = 0.0; + m.maxZ = 0.3; + m.z = 0.0; + m_multiGridmap.push_back(m); + + m.name = "projected_spine_map"; + m.minZ = 0.25; + m.maxZ = 1.4; + m.z = 0.6; + m_multiGridmap.push_back(m); + + m.name = "projected_arm_map"; + m.minZ = 0.7; + m.maxZ = 0.9; + m.z = 0.8; + m_multiGridmap.push_back(m); + + + for (unsigned i = 0; i < m_multiGridmap.size(); ++i){ + ros::Publisher* pub = new ros::Publisher(m_nh.advertise(m_multiGridmap.at(i).name, 5, m_latchedTopics)); + m_multiMapPub.push_back(pub); + } + + // init arm links (could be params as well) + m_armLinks.push_back("l_elbow_flex_link"); + m_armLinkOffsets.push_back(0.10); + m_armLinks.push_back("l_gripper_l_finger_tip_link"); + m_armLinkOffsets.push_back(0.03); + m_armLinks.push_back("l_gripper_r_finger_tip_link"); + m_armLinkOffsets.push_back(0.03); + m_armLinks.push_back("l_upper_arm_roll_link"); + m_armLinkOffsets.push_back(0.16); + m_armLinks.push_back("l_wrist_flex_link"); + m_armLinkOffsets.push_back(0.05); + m_armLinks.push_back("r_elbow_flex_link"); + m_armLinkOffsets.push_back(0.10); + m_armLinks.push_back("r_gripper_l_finger_tip_link"); + m_armLinkOffsets.push_back(0.03); + m_armLinks.push_back("r_gripper_r_finger_tip_link"); + m_armLinkOffsets.push_back(0.03); + m_armLinks.push_back("r_upper_arm_roll_link"); + m_armLinkOffsets.push_back(0.16); + m_armLinks.push_back("r_wrist_flex_link"); + m_armLinkOffsets.push_back(0.05); + + +} + +OctomapServerMultilayer::~OctomapServerMultilayer(){ + for (unsigned i = 0; i < m_multiMapPub.size(); ++i){ + delete m_multiMapPub[i]; + } + +} + +void OctomapServerMultilayer::handlePreNodeTraversal(const ros::Time& rostime){ + // multilayer server always publishes 2D maps: + m_publish2DMap = true; + nav_msgs::MapMetaData gridmapInfo = m_gridmap.info; + + OctomapServer::handlePreNodeTraversal(rostime); + + + // recalculate height of arm layer (stub, TODO) + geometry_msgs::PointStamped vin; + vin.point.x = 0; + vin.point.y = 0; + vin.point.z = 0; + vin.header.stamp = rostime; + double link_padding = 0.03; + + double minArmHeight = 2.0; + double maxArmHeight = 0.0; + + for (unsigned i = 0; i < m_armLinks.size(); ++i){ + vin.header.frame_id = m_armLinks[i]; + geometry_msgs::PointStamped vout; + const bool found_trans = + m_tfListener.waitForTransform("base_footprint", m_armLinks.at(i), + ros::Time(0), ros::Duration(1.0)); + ROS_ASSERT_MSG(found_trans, "Timed out waiting for transform to %s", + m_armLinks[i].c_str()); + m_tfListener.transformPoint("base_footprint",vin,vout); + maxArmHeight = std::max(maxArmHeight, vout.point.z + (m_armLinkOffsets.at(i) + link_padding)); + minArmHeight = std::min(minArmHeight, vout.point.z - (m_armLinkOffsets.at(i) + link_padding)); + } + ROS_INFO("Arm layer interval adjusted to (%f,%f)", minArmHeight, maxArmHeight); + m_multiGridmap.at(2).minZ = minArmHeight; + m_multiGridmap.at(2).maxZ = maxArmHeight; + m_multiGridmap.at(2).z = (maxArmHeight+minArmHeight)/2.0; + + + + + + // TODO: also clear multilevel maps in BBX region (see OctomapServer.cpp)? + + bool mapInfoChanged = mapChanged(gridmapInfo, m_gridmap.info); + + for (MultilevelGrid::iterator it = m_multiGridmap.begin(); it != m_multiGridmap.end(); ++it){ + it->map.header = m_gridmap.header; + it->map.info = m_gridmap.info; + it->map.info.origin.position.z = it->z; + if (m_projectCompleteMap){ + ROS_INFO("Map resolution changed, rebuilding complete 2D maps"); + it->map.data.clear(); + // init to unknown: + it->map.data.resize(it->map.info.width * it->map.info.height, -1); + } else if (mapInfoChanged){ + adjustMapData(it->map, gridmapInfo); + } + } +} + +void OctomapServerMultilayer::handlePostNodeTraversal(const ros::Time& rostime){ + + // TODO: calc tall / short obs. cells for arm layer, => temp arm layer +// std::vector shortObsCells; +// for(unsigned int i=0; i 0.8) +// arm_map.data[i] = 101; +// else{ +// arm_map.data[i] = 100; +// shortObsCells.push_back(i); +// } +// } +// +// std::vector tallObsCells; +// tallObsCells.reserve(shortObsCells.size()); +// 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}; +// for(unsigned int i=0; i=arm_map.data.size()) +// continue; +// if(arm_map.data[temp]==101){ +// tallObsCells.push_back(shortObsCells[i]); +// break; +// } +// } +// } +// for(unsigned int i=0; ipublish(m_multiGridmap.at(i).map); + } + +} +void OctomapServerMultilayer::update2DMap(const OcTreeT::iterator& it, bool occupied){ + double z = it.getZ(); + double s2 = it.getSize()/2.0; + + // create a mask on which maps to update: + std::vector inMapLevel(m_multiGridmap.size(), false); + for (unsigned i = 0; i < m_multiGridmap.size(); ++i){ + if (z+s2 >= m_multiGridmap[i].minZ && z-s2 <= m_multiGridmap[i].maxZ){ + inMapLevel[i] = true; + } + } + + if (it.getDepth() == m_maxTreeDepth){ + unsigned idx = mapIdx(it.getKey()); + if (occupied) + m_gridmap.data[idx] = 100; + else if (m_gridmap.data[idx] == -1){ + m_gridmap.data[idx] = 0; + } + + for (unsigned i = 0; i < inMapLevel.size(); ++i){ + if (inMapLevel[i]){ + if (occupied) + m_multiGridmap[i].map.data[idx] = 100; + else if (m_multiGridmap[i].map.data[idx] == -1) + m_multiGridmap[i].map.data[idx] = 0; + } + } + + } else { + int intSize = 1 << (m_treeDepth - it.getDepth()); + octomap::OcTreeKey minKey=it.getIndexKey(); + for(int dx=0; dx < intSize; dx++){ + int i = (minKey[0]+dx - m_paddedMinKey[0])/m_multires2DScale; + for(int dy=0; dy < intSize; dy++){ + unsigned idx = mapIdx(i, (minKey[1]+dy - m_paddedMinKey[1])/m_multires2DScale); + if (occupied) + m_gridmap.data[idx] = 100; + else if (m_gridmap.data[idx] == -1){ + m_gridmap.data[idx] = 0; + } + + for (unsigned i = 0; i < inMapLevel.size(); ++i){ + if (inMapLevel[i]){ + if (occupied) + m_multiGridmap[i].map.data[idx] = 100; + else if (m_multiGridmap[i].map.data[idx] == -1) + m_multiGridmap[i].map.data[idx] = 0; + } + } + } + } + } + + +} + +} + + + diff --git a/third_party/octomap_server/src/TrackingOctomapServer.cpp b/third_party/octomap_server/src/TrackingOctomapServer.cpp new file mode 100644 index 0000000..0b0090e --- /dev/null +++ b/third_party/octomap_server/src/TrackingOctomapServer.cpp @@ -0,0 +1,155 @@ +/* + * Copyright (c) 2012, D. Kuhner, P. Ruchti, University of Freiburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +using namespace octomap; + +namespace octomap_server { + +TrackingOctomapServer::TrackingOctomapServer(const std::string& filename) : + OctomapServer() +{ + //read tree if necessary + if (filename != "") { + if (m_octree->readBinary(filename)) { + ROS_INFO("Octomap file %s loaded (%zu nodes).", filename.c_str(), m_octree->size()); + m_treeDepth = m_octree->getTreeDepth(); + m_res = m_octree->getResolution(); + m_gridmap.info.resolution = m_res; + + publishAll(); + } else { + ROS_ERROR("Could not open requested file %s, exiting.", filename.c_str()); + exit(-1); + } + } + + ros::NodeHandle private_nh("~"); + + std::string changeSetTopic = "changes"; + std::string changeIdFrame = "/talker/changes"; + + private_nh.param("topic_changes", changeSetTopic, changeSetTopic); + private_nh.param("change_id_frame", change_id_frame, changeIdFrame); + private_nh.param("track_changes", track_changes, false); + private_nh.param("listen_changes", listen_changes, false); + private_nh.param("min_change_pub", min_change_pub, 0); + + if (track_changes && listen_changes) { + ROS_WARN("OctoMapServer: It might not be useful to publish changes and at the same time listen to them." + "Setting 'track_changes' to false!"); + track_changes = false; + } + + if (track_changes) { + ROS_INFO("starting server"); + pubChangeSet = private_nh.advertise( + changeSetTopic, 1); + m_octree->enableChangeDetection(true); + } + + if (listen_changes) { + ROS_INFO("starting client"); + subChangeSet = private_nh.subscribe(changeSetTopic, 1, + &TrackingOctomapServer::trackCallback, this); + } +} + +TrackingOctomapServer::~TrackingOctomapServer() { +} + +void TrackingOctomapServer::insertScan(const tf::Point & sensorOrigin, const PCLPointCloud & ground, const PCLPointCloud & nonground) { + OctomapServer::insertScan(sensorOrigin, ground, nonground); + + if (track_changes) { + trackChanges(); + } +} + +void TrackingOctomapServer::trackChanges() { + KeyBoolMap::const_iterator startPnt = m_octree->changedKeysBegin(); + KeyBoolMap::const_iterator endPnt = m_octree->changedKeysEnd(); + + pcl::PointCloud changedCells = pcl::PointCloud(); + + int c = 0; + for (KeyBoolMap::const_iterator iter = startPnt; iter != endPnt; ++iter) { + ++c; + OcTreeNode* node = m_octree->search(iter->first); + + bool occupied = m_octree->isNodeOccupied(node); + + point3d center = m_octree->keyToCoord(iter->first); + + pcl::PointXYZI pnt; + pnt.x = center(0); + pnt.y = center(1); + pnt.z = center(2); + + if (occupied) { + pnt.intensity = 1000; + } + else { + pnt.intensity = -1000; + } + + changedCells.push_back(pnt); + } + + if (c > min_change_pub) + { + sensor_msgs::PointCloud2 changed; + pcl::toROSMsg(changedCells, changed); + changed.header.frame_id = change_id_frame; + changed.header.stamp = ros::Time().now(); + pubChangeSet.publish(changed); + ROS_DEBUG("[server] sending %d changed entries", (int)changedCells.size()); + + m_octree->resetChangeDetection(); + ROS_DEBUG("[server] octomap size after updating: %d", (int)m_octree->calcNumNodes()); + } +} + +void TrackingOctomapServer::trackCallback(sensor_msgs::PointCloud2Ptr cloud) { + pcl::PointCloud cells; + pcl::fromROSMsg(*cloud, cells); + ROS_DEBUG("[client] size of newly occupied cloud: %i", (int)cells.points.size()); + + for (size_t i = 0; i < cells.points.size(); i++) { + pcl::PointXYZI& pnt = cells.points[i]; + m_octree->updateNode(m_octree->coordToKey(pnt.x, pnt.y, pnt.z), pnt.intensity, false); + } + + m_octree->updateInnerOccupancy(); + ROS_DEBUG("[client] octomap size after updating: %d", (int)m_octree->calcNumNodes()); +} + +} /* namespace octomap */ diff --git a/third_party/octomap_server/src/octomap_saver.cpp b/third_party/octomap_server/src/octomap_saver.cpp new file mode 100644 index 0000000..99194fe --- /dev/null +++ b/third_party/octomap_server/src/octomap_saver.cpp @@ -0,0 +1,124 @@ +/* + * Copyright (c) 2010-2013, A. Hornung, University of Freiburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +#include +using octomap_msgs::GetOctomap; + +#define USAGE "\nUSAGE: octomap_saver [-f] \n" \ + " -f: Query for the full occupancy octree, instead of just the compact binary one\n" \ + " mapfile.bt: filename of map to be saved (.bt: binary tree, .ot: general octree)\n" + +using namespace std; +using namespace octomap; + +class MapSaver{ +public: + MapSaver(const std::string& mapname, bool full){ + ros::NodeHandle n; + std::string servname = "octomap_binary"; + if (full) + servname = "octomap_full"; + ROS_INFO("Requesting the map from %s...", n.resolveName(servname).c_str()); + GetOctomap::Request req; + GetOctomap::Response resp; + while(n.ok() && !ros::service::call(servname, req, resp)) + { + ROS_WARN("Request to %s failed; trying again...", n.resolveName(servname).c_str()); + usleep(1000000); + } + + if (n.ok()){ // skip when CTRL-C + + AbstractOcTree* tree = octomap_msgs::msgToMap(resp.map); + AbstractOccupancyOcTree* octree = NULL; + if (tree){ + octree = dynamic_cast(tree); + } else { + ROS_ERROR("Error creating octree from received message"); + if (resp.map.id == "ColorOcTree") + ROS_WARN("You requested a binary map for a ColorOcTree - this is currently not supported. Please add -f to request a full map"); + } + + if (octree){ + ROS_INFO("Map received (%zu nodes, %f m res), saving to %s", octree->size(), octree->getResolution(), mapname.c_str()); + + std::string suffix = mapname.substr(mapname.length()-3, 3); + if (suffix== ".bt"){ // write to binary file: + if (!octree->writeBinary(mapname)){ + ROS_ERROR("Error writing to file %s", mapname.c_str()); + } + } else if (suffix == ".ot"){ // write to full .ot file: + if (!octree->write(mapname)){ + ROS_ERROR("Error writing to file %s", mapname.c_str()); + } + } else{ + ROS_ERROR("Unknown file extension, must be either .bt or .ot"); + } + + + } else{ + ROS_ERROR("Error reading OcTree from stream"); + } + + delete octree; + + } + } +}; + +int main(int argc, char** argv){ + ros::init(argc, argv, "octomap_saver"); + std::string mapFilename(""); + bool fullmap = false; + if (argc == 3 && strcmp(argv[1], "-f")==0){ + fullmap = true; + mapFilename = std::string(argv[2]); + } else if (argc == 2) + mapFilename = std::string(argv[1]); + else{ + ROS_ERROR("%s", USAGE); + exit(1); + } + + try{ + MapSaver ms(mapFilename, fullmap); + }catch(std::runtime_error& e){ + ROS_ERROR("octomap_saver exception: %s", e.what()); + exit(2); + } + + exit(0); +} + + diff --git a/third_party/octomap_server/src/octomap_server_multilayer.cpp b/third_party/octomap_server/src/octomap_server_multilayer.cpp new file mode 100644 index 0000000..cf87400 --- /dev/null +++ b/third_party/octomap_server/src/octomap_server_multilayer.cpp @@ -0,0 +1,71 @@ +/* + * Copyright (c) 2009-2013, A. Hornung, University of Freiburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#include +#include + +#define USAGE "\nUSAGE: octomap_server_multilayer \n" \ + " map.bt: inital octomap 3D map file to read\n" + +using namespace octomap_server; + +int main(int argc, char** argv){ + ros::init(argc, argv, "octomap_server_multilayer"); + std::string mapFilename(""); + + if (argc > 2 || (argc == 2 && std::string(argv[1]) == "-h")){ + ROS_ERROR("%s", USAGE); + exit(-1); + } + + + OctomapServerMultilayer server; + ros::spinOnce(); + + if (argc == 2){ + mapFilename = std::string(argv[1]); + if (!server.openFile(mapFilename)){ + ROS_ERROR("Could not open file %s", mapFilename.c_str()); + exit(1); + } + } + + + + + try{ + ros::spin(); + }catch(std::runtime_error& e){ + ROS_ERROR("octomap_server_multilayer exception: %s", e.what()); + return -1; + } + + return 0; +} diff --git a/third_party/octomap_server/src/octomap_server_node.cpp b/third_party/octomap_server/src/octomap_server_node.cpp new file mode 100644 index 0000000..b3c5dda --- /dev/null +++ b/third_party/octomap_server/src/octomap_server_node.cpp @@ -0,0 +1,88 @@ +/** +* octomap_server: A Tool to serve 3D OctoMaps in ROS (binary and as visualization) +* (inspired by the ROS map_saver) +* @author A. Hornung, University of Freiburg, Copyright (C) 2009 - 2012. +* @see http://octomap.sourceforge.net/ +* License: BSD +*/ + +/* + * Copyright (c) 2009-2012, A. Hornung, University of Freiburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#include +#include + +#define USAGE "\nUSAGE: octomap_server \n" \ + " map.bt: inital octomap 3D map file to read\n" + +using namespace octomap_server; + +int main(int argc, char** argv){ + ros::init(argc, argv, "octomap_server"); + const ros::NodeHandle nh; + const ros::NodeHandle private_nh("~"); + std::string mapFilename(""), mapFilenameParam(""); + + if (argc > 2 || (argc == 2 && std::string(argv[1]) == "-h")){ + ROS_ERROR("%s", USAGE); + exit(-1); + } + + OctomapServer server(private_nh, nh); + ros::spinOnce(); + + if (argc == 2){ + mapFilename = std::string(argv[1]); + } + + if (private_nh.getParam("map_file", mapFilenameParam)) { + if (mapFilename != "") { + 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()); + } else { + mapFilename = mapFilenameParam; + } + } + + if (mapFilename != "") { + if (!server.openFile(mapFilename)){ + ROS_ERROR("Could not open file %s", mapFilename.c_str()); + exit(1); + } + } + + try{ + ros::spin(); + }catch(std::runtime_error& e){ + ROS_ERROR("octomap_server exception: %s", e.what()); + return -1; + } + + return 0; +} diff --git a/third_party/octomap_server/src/octomap_server_nodelet.cpp b/third_party/octomap_server/src/octomap_server_nodelet.cpp new file mode 100644 index 0000000..dfe09d6 --- /dev/null +++ b/third_party/octomap_server/src/octomap_server_nodelet.cpp @@ -0,0 +1,69 @@ +/** +* octomap_server_nodelet: A nodelet version of A. Hornung's octomap server +* @author Marcus Liebhardt +* License: BSD +*/ + +/* + * Copyright (c) 2012, Marcus Liebhardt, Yujin Robot + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of Yujin Robot nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#include +#include +#include +#include + + +namespace octomap_server +{ + +class OctomapServerNodelet : public nodelet::Nodelet +{ +public: + virtual void onInit() + { + NODELET_DEBUG("Initializing octomap server nodelet ..."); + ros::NodeHandle& nh = this->getNodeHandle(); + ros::NodeHandle& private_nh = this->getPrivateNodeHandle(); + server_.reset(new OctomapServer(private_nh, nh)); + + std::string mapFilename(""); + if (private_nh.getParam("map_file", mapFilename)) { + if (!server_->openFile(mapFilename)){ + NODELET_WARN("Could not open file %s", mapFilename.c_str()); + } + } + } +private: + boost::shared_ptr server_; +}; + +} // namespace + +PLUGINLIB_EXPORT_CLASS(octomap_server::OctomapServerNodelet, nodelet::Nodelet) diff --git a/third_party/octomap_server/src/octomap_server_static.cpp b/third_party/octomap_server/src/octomap_server_static.cpp new file mode 100644 index 0000000..10bec47 --- /dev/null +++ b/third_party/octomap_server/src/octomap_server_static.cpp @@ -0,0 +1,156 @@ +/* + * Copyright (c) 2013, A. Hornung, University of Freiburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +#include +using octomap_msgs::GetOctomap; + +#define USAGE "\nUSAGE: octomap_server_static \n" \ + " mapfile.bt: OctoMap filename to be loaded (.bt: binary tree, .ot: general octree)\n" + +using namespace std; +using namespace octomap; + +class OctomapServerStatic{ +public: + OctomapServerStatic(const std::string& filename) + : m_octree(NULL), m_worldFrameId("/map") + { + + ros::NodeHandle private_nh("~"); + private_nh.param("frame_id", m_worldFrameId, m_worldFrameId); + + + // open file: + if (filename.length() <= 3){ + ROS_ERROR("Octree file does not have .ot extension"); + exit(1); + } + + std::string suffix = filename.substr(filename.length()-3, 3); + + // .bt files only as OcTree, all other classes need to be in .ot files: + if (suffix == ".bt"){ + OcTree* octree = new OcTree(filename); + + m_octree = octree; + } else if (suffix == ".ot"){ + AbstractOcTree* tree = AbstractOcTree::read(filename); + if (!tree){ + ROS_ERROR("Could not read octree from file"); + exit(1); + } + + m_octree = dynamic_cast(tree); + + } else{ + ROS_ERROR("Octree file does not have .bt or .ot extension"); + exit(1); + } + + if (!m_octree ){ + ROS_ERROR("Could not read right octree class in file"); + exit(1); + } + + ROS_INFO("Read octree type \"%s\" from file %s", m_octree->getTreeType().c_str(), filename.c_str()); + ROS_INFO("Octree resultion: %f, size: %zu", m_octree->getResolution(), m_octree->size()); + + + m_octomapBinaryService = m_nh.advertiseService("octomap_binary", &OctomapServerStatic::octomapBinarySrv, this); + m_octomapFullService = m_nh.advertiseService("octomap_full", &OctomapServerStatic::octomapFullSrv, this); + + } + + ~OctomapServerStatic(){ + + + } + + bool octomapBinarySrv(GetOctomap::Request &req, + GetOctomap::Response &res) + { + ROS_INFO("Sending binary map data on service request"); + res.map.header.frame_id = m_worldFrameId; + res.map.header.stamp = ros::Time::now(); + if (!octomap_msgs::binaryMapToMsg(*m_octree, res.map)) + return false; + + return true; + } + + bool octomapFullSrv(GetOctomap::Request &req, + GetOctomap::Response &res) + { + ROS_INFO("Sending full map data on service request"); + res.map.header.frame_id = m_worldFrameId; + res.map.header.stamp = ros::Time::now(); + + + if (!octomap_msgs::fullMapToMsg(*m_octree, res.map)) + return false; + + return true; + } + +private: + ros::ServiceServer m_octomapBinaryService, m_octomapFullService; + ros::NodeHandle m_nh; + std::string m_worldFrameId; + AbstractOccupancyOcTree* m_octree; + +}; + +int main(int argc, char** argv){ + ros::init(argc, argv, "octomap_server_static"); + std::string mapFilename(""); + + if (argc == 2) + mapFilename = std::string(argv[1]); + else{ + ROS_ERROR("%s", USAGE); + exit(1); + } + + try{ + OctomapServerStatic ms(mapFilename); + ros::spin(); + }catch(std::runtime_error& e){ + ROS_ERROR("octomap_server_static exception: %s", e.what()); + exit(2); + } + + exit(0); +} + + diff --git a/third_party/octomap_server/src/octomap_tracking_server_node.cpp b/third_party/octomap_server/src/octomap_tracking_server_node.cpp new file mode 100644 index 0000000..6acce66 --- /dev/null +++ b/third_party/octomap_server/src/octomap_tracking_server_node.cpp @@ -0,0 +1,59 @@ +/* + * Copyright (c) 2012, D. Kuhner, P. Ruchti, University of Freiburg + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the University of Freiburg nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include + +#define USAGE "\nUSAGE: octomap_tracking_server \n" \ + " map.bt: octomap 3D map file to read\n" + +using namespace octomap_server; + +int main(int argc, char** argv){ + ros::init(argc, argv, "octomap_tracking_server"); + std::string mapFilename(""); + + if (argc > 2 || (argc == 2 && std::string(argv[1]) == "-h")){ + ROS_ERROR("%s", USAGE); + exit(-1); + } + + if (argc == 2) + mapFilename = std::string(argv[1]); + + try{ + TrackingOctomapServer ms(mapFilename); + ros::spin(); + }catch(std::runtime_error& e){ + ROS_ERROR("octomap_server exception: %s", e.what()); + return -1; + } + + return 0; +} From 17c66488fc28c66fd93908d87572ef7ecaea050a Mon Sep 17 00:00:00 2001 From: youngjae Date: Wed, 1 Nov 2023 09:11:12 -0400 Subject: [PATCH 2/4] path searching package create & datatype define --- path_searching/CMakeLists.txt | 206 +++++++++++++++++++ path_searching/include/jps_basis/data_type.h | 88 ++++++++ path_searching/package.xml | 68 ++++++ 3 files changed, 362 insertions(+) create mode 100644 path_searching/CMakeLists.txt create mode 100644 path_searching/include/jps_basis/data_type.h create mode 100644 path_searching/package.xml diff --git a/path_searching/CMakeLists.txt b/path_searching/CMakeLists.txt new file mode 100644 index 0000000..89fb1da --- /dev/null +++ b/path_searching/CMakeLists.txt @@ -0,0 +1,206 @@ +cmake_minimum_required(VERSION 3.0.2) +project(path_searching) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES path_searching +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/path_searching.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/path_searching_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_path_searching.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/path_searching/include/jps_basis/data_type.h b/path_searching/include/jps_basis/data_type.h new file mode 100644 index 0000000..c3f5cac --- /dev/null +++ b/path_searching/include/jps_basis/data_type.h @@ -0,0 +1,88 @@ +/** +Define data types used in this library +Mostly aliasing from Eigen Library +**/ + +#include +#include +#include +#include + +#ifndef DATA_TYPE_H +#define DATA_TYPE_H + +typedef double decimal_t; + +//Pre-allocated std::vector for Eigen using vec_E +template +using vec_E = std::vector>; + +//Eigen::Matrix +//Eigen 1D float vector of size N +template +using Vecf = Eigen::Matrix; +//Eigen 1D int vector of size N +template +using Veci = Eigen::Matrix; +//Eigen 1D float vector of dynamic size +using VecDf = Eigen::Matrix; +//MxN Eigen matrix +template +using Matf = Eigen::Matrix; +//MxN Eigen matrix with M unknown +template +using MatDNf = Eigen::Matrix; +//Vector of Eigen 1D float vector +template +using vec_Vecf = vec_E>; +//Vector of Eigen 1D int vector +template +using vec_Veci = vec_E>; + +//Eigen 1D float vector of size 2 +typedef Vecf<2> Vec2f; +//Eigen 1D int vector of size 2 +typedef Veci<2> Vec2i; +//Eigen 1D float vector of size 3 +typedef Vecf<3> Vec3f; +//Eigen 1D int vector of size 3 +typedef Veci<3> Vec3i; +//Eigen 1D float vector of size 4 +typedef Vecf<4> Vec4f; +//Column vector in float of size 6 +typedef Vecf<6> Vec6f; + +//Vector of type Vec2f. +typedef vec_E vec_Vec2f; +//Vector of type Vec2i. +typedef vec_E vec_Vec2i; +//Vector of type Vec3f. +typedef vec_E vec_Vec3f; +//Vector of type Vec3i. +typedef vec_E vec_Vec3i; + +//2x2 Matrix in float +typedef Matf<2, 2> Mat2f; +//3x3 Matrix in float +typedef Matf<3, 3> Mat3f; +//4x4 Matrix in float +typedef Matf<4, 4> Mat4f; +//6x6 Matrix in float +typedef Matf<6, 6> Mat6f; + +//Dynamic Nx1 Eigen float vector +typedef Vecf VecDf; +//Mx3 Eigen float matrix +typedef MatDNf<3> MatD3f; +//Dynamic MxN Eigen float matrix +typedef Matf MatDf; + +//Allias of Eigen::Affine2d +typedef Eigen::Transform Aff2f; +//Allias of Eigen::Affine3d +typedef Eigen::Transform Aff3f; + + +typedef Byungsin::PeePeeman Ass3s; + +#endif \ No newline at end of file diff --git a/path_searching/package.xml b/path_searching/package.xml new file mode 100644 index 0000000..e1b3558 --- /dev/null +++ b/path_searching/package.xml @@ -0,0 +1,68 @@ + + + path_searching + 0.0.0 + The path_searching package + + + + + yjkim + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + From bddaf4ac6f76660387ee73cf909536817a2f335c Mon Sep 17 00:00:00 2001 From: youngjae Date: Wed, 1 Nov 2023 09:12:44 -0400 Subject: [PATCH 3/4] path searching package create & datatype define --- path_searching/include/jps_basis/data_type.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/path_searching/include/jps_basis/data_type.h b/path_searching/include/jps_basis/data_type.h index c3f5cac..5d47a03 100644 --- a/path_searching/include/jps_basis/data_type.h +++ b/path_searching/include/jps_basis/data_type.h @@ -82,7 +82,4 @@ typedef Eigen::Transform Aff2f; //Allias of Eigen::Affine3d typedef Eigen::Transform Aff3f; - -typedef Byungsin::PeePeeman Ass3s; - #endif \ No newline at end of file From e9aa07ffb821ec67fc11e0f5d21bb48c662af9f2 Mon Sep 17 00:00:00 2001 From: youngjae Date: Wed, 1 Nov 2023 12:11:44 -0400 Subject: [PATCH 4/4] map_util class basic member&method define --- .../include/jps_collision/map_util.h | 47 +++++++++++++++++++ path_searching/package.xml | 2 +- 2 files changed, 48 insertions(+), 1 deletion(-) create mode 100644 path_searching/include/jps_collision/map_util.h diff --git a/path_searching/include/jps_collision/map_util.h b/path_searching/include/jps_collision/map_util.h new file mode 100644 index 0000000..452d548 --- /dev/null +++ b/path_searching/include/jps_collision/map_util.h @@ -0,0 +1,47 @@ +#ifndef JPS_MAP_UTIL_H +#define JPS_MAP_UTIL_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace Eigen; + +namespace JPS { + //Type ailas -> 1D array map data as Tmap name + using Tmap = std::vector; + + //MapUtil template class for collision checking + template class MapUtil{ + + public: + + MapUtil() {} //simple constructor + Tmap getMap() { return map_; } // get map data + bool has_map_() { return has_map;} // check map exist + decimal_t getRes() { return res_; } // get resolution + Veci getDim() { return dim_; } // get dimensions + Vecf getOrigin() { return origin_d_; } // get origin + + + }; + + typedef MapUtil<2> OccMapUtil; + typedef MapUtil<3> VoxelMapUtil; +} + +#endif \ No newline at end of file diff --git a/path_searching/package.xml b/path_searching/package.xml index e1b3558..9800bec 100644 --- a/path_searching/package.xml +++ b/path_searching/package.xml @@ -7,7 +7,7 @@ - yjkim + yjkim