├── .github ├── travis.sh └── workflows │ └── ci.yml ├── .gitignore ├── CMakeLists.txt ├── README.md ├── deps.repos ├── include └── octomap_server2 │ ├── conversions.h │ ├── octomap_server.hpp │ └── transforms.hpp ├── launch └── octomap_server_launch.py ├── package.xml └── src ├── conversions.cpp ├── octomap_server.cpp └── transforms.cpp /.github/travis.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | echo 'debconf debconf/frontend select Noninteractive' | sudo debconf-set-selections 4 | 5 | apt-get update && apt-get install -y \ 6 | libflann-dev \ 7 | liboctomap-dev \ 8 | libpcl-dev 9 | 10 | if [ -z ${CI_SOURCE_DIR} ]; 11 | then 12 | (cd ${CI_SOURCE_DIR}; git log --oneline | head -10) 13 | fi 14 | 15 | if [ -z ${REPO_NAME} ]; 16 | then 17 | export REPO_NAME=octomap_server2 18 | fi 19 | 20 | if [ -z ${CI_SOURCE_PATH} ]; 21 | then 22 | export CI_SOURCE_PATH=/home/src/$REPO_NAME 23 | fi 24 | 25 | export WORKDIR=${HOME}/ros2/${ROS_DISTRO}/src 26 | 27 | if [ ! -d $WORKDIR ]; 28 | then 29 | mkdir -p $WORKDIR 30 | fi 31 | 32 | cd $WORKDIR/.. 33 | mkdir -p src/$REPO_NAME 34 | cp -r ${CI_SOURCE_PATH} src/ 35 | 36 | echo `pwd` 37 | ls 38 | echo $CI_SOURCE_PATH 39 | 40 | vcs import ./src/$REPO_NAME < src/${REPO_NAME}/deps.repos 41 | colcon build --symlink-install 42 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: build 2 | 3 | on: 4 | pull_request: 5 | push: 6 | branches: 7 | - ci 8 | 9 | env: 10 | ROS_DISTRO: foxy 11 | ROS_VERSION: ros2 12 | DEBIAN_FRONTEND: noninteractive 13 | 14 | defaults: 15 | run: 16 | shell: bash 17 | 18 | jobs: 19 | build: 20 | runs-on: ubuntu-20.04 21 | 22 | steps: 23 | - name: Checkout 24 | uses: actions/checkout@v2 25 | 26 | - name: Using Docker 27 | run: | 28 | export DOCKER_IMAGE=ros:humble-perception-jammy 29 | export CI_SOURCE_PATH=$(pwd) 30 | export REPO_NAME=${{ github.event.repository.name }} 31 | export HOME=$GITHUB_WORKSPACE 32 | docker run -v $HOME:$HOME -e CI_SOURCE_PATH -e $REPO_NAME -e HOME $DOCKER_IMAGE bash -c 'cd $CI_SOURCE_PATH; source .github/travis.sh' 33 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | install/ 3 | log/ 4 | /tests/clar.suite 5 | /tests/clar.suite.rule 6 | /tests/.clarcache 7 | /apidocs 8 | /trash-*.exe 9 | /libgit2.pc 10 | /config.mak 11 | *.o 12 | *.a 13 | *.exe 14 | *.gcda 15 | *.gcno 16 | *.gcov 17 | .lock-wafbuild 18 | .waf* 19 | build/ 20 | build-amiga/ 21 | tests/tmp/ 22 | msvc/Debug/ 23 | msvc/Release/ 24 | *.sln 25 | *.suo 26 | *.vc*proj* 27 | *.sdf 28 | *.opensdf 29 | *.aps 30 | !cmake/Modules/*.cmake 31 | .DS_Store 32 | *~ 33 | .*.swp 34 | tags 35 | mkmf.log 36 | __pycache__/* 37 | *.pyc -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(octomap_server2) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | find_package(ament_cmake REQUIRED) 19 | find_package(rclcpp REQUIRED) 20 | find_package(rclcpp_components REQUIRED) 21 | find_package(PCL 1.10 REQUIRED) 22 | find_package(sensor_msgs REQUIRED) 23 | find_package(std_msgs REQUIRED) 24 | find_package(nav_msgs REQUIRED) 25 | find_package(geometry_msgs REQUIRED) 26 | find_package(visualization_msgs REQUIRED) 27 | find_package(std_srvs REQUIRED) 28 | find_package(OpenMP REQUIRED) 29 | find_package(pcl_conversions REQUIRED) 30 | find_package(message_filters REQUIRED) 31 | find_package(octomap REQUIRED) 32 | find_package(octomap_msgs REQUIRED) 33 | find_package(tf2 REQUIRED) 34 | find_package(tf2_ros REQUIRED) 35 | find_package(tf2_msgs REQUIRED) 36 | find_package(tf2_geometry_msgs REQUIRED) 37 | find_package(rclcpp_components REQUIRED) 38 | 39 | link_directories(${PCL_LIBRARY_DIRS}) 40 | include_directories(${PCL_INCLUDE_DIRS}) 41 | include_directories(include) 42 | 43 | add_library(octomap_server2 SHARED 44 | src/octomap_server.cpp 45 | src/transforms.cpp 46 | src/conversions.cpp 47 | ) 48 | 49 | ament_target_dependencies(octomap_server2 50 | rclcpp 51 | PCL 52 | pcl_conversions 53 | sensor_msgs 54 | std_msgs 55 | nav_msgs 56 | visualization_msgs 57 | geometry_msgs 58 | std_srvs 59 | octomap 60 | octomap_msgs 61 | message_filters 62 | tf2_ros 63 | tf2_msgs 64 | tf2 65 | tf2_geometry_msgs 66 | rclcpp_components 67 | ) 68 | 69 | target_link_libraries(octomap_server2 ${PCL_LIBRARIES} ${OCTOMAP_LIBRARIES}) 70 | 71 | rclcpp_components_register_node(octomap_server2 72 | PLUGIN "octomap_server::OctomapServer" 73 | EXECUTABLE octomap_server) 74 | 75 | install(TARGETS 76 | octomap_server2 77 | ARCHIVE DESTINATION lib 78 | LIBRARY DESTINATION lib 79 | RUNTIME DESTINATION bin) 80 | 81 | install(DIRECTORY 82 | launch 83 | DESTINATION share/${PROJECT_NAME}/ 84 | ) 85 | 86 | ament_package() 87 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |
2 |

OctoMap Server2

3 |

Implementation of octomap for ROS2.0

4 | 5 |
6 | 7 | Port of the ROS1 [octomap server](https://github.com/OctoMap/octomap_mapping) for ROS2.0 8 | 9 | #### Installation 10 | Firstly make sure you have [octomap](https://github.com/OctoMap/octomap.git) installed on your system 11 | 12 | Next, clone this ros package to the appropriate ros2 workspace 13 | ```bash 14 | $ git clone https://github.com/iKrishneel/octomap_server2.git 15 | ``` 16 | Clone the dependency repositories to the workspace 17 | ```bash 18 | # will clone octomap_msgs to the workspace 19 | $ vcs import . < deps.repos 20 | ``` 21 | 22 | #### Building 23 | Use colcon to build the workspace 24 | ```bash 25 | $ colcon build --symlink-install --packages-select octomap_msgs octomap_server2 26 | ``` 27 | 28 | #### Running 29 | Launch the node with appropriate input on topic `cloud_in` 30 | ```bash 31 | $ ros2 launch octomap_server2 octomap_server_launch.py 32 | ``` -------------------------------------------------------------------------------- /deps.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ./../octomap_msgs: 3 | type: git 4 | url: https://github.com/OctoMap/octomap_msgs.git 5 | version: ros2 6 | ./../perception_pcl: 7 | type: git 8 | url: https://github.com/ros-perception/perception_pcl.git 9 | version: ros2 10 | ./../pcl_msgs: 11 | type: git 12 | url: https://github.com/ros-perception/pcl_msgs.git 13 | version: ros2 -------------------------------------------------------------------------------- /include/octomap_server2/conversions.h: -------------------------------------------------------------------------------- 1 | /** 2 | * OctoMap ROS integration 3 | * 4 | * @author A. Hornung, University of Freiburg, Copyright (C) 2011-2012. 5 | * @see http://www.ros.org/wiki/octomap_ros 6 | * License: BSD 7 | */ 8 | 9 | /* 10 | * Copyright (c) 2011, A. Hornung, University of Freiburg 11 | * All rights reserved. 12 | * 13 | * Redistribution and use in source and binary forms, with or without 14 | * modification, are permitted provided that the following conditions are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above copyright 19 | * notice, this list of conditions and the following disclaimer in the 20 | * documentation and/or other materials provided with the distribution. 21 | * * Neither the name of the University of Freiburg nor the names of its 22 | * contributors may be used to endorse or promote products derived from 23 | * this software without specific prior written permission. 24 | * 25 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 26 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 27 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 28 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 29 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 30 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 31 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 32 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 33 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 34 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | */ 37 | 38 | #ifndef OCTOMAP_ROS_CONVERSIONS_H 39 | #define OCTOMAP_ROS_CONVERSIONS_H 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | #include 50 | #include 51 | 52 | namespace octomap { 53 | /** 54 | * @brief Conversion from octomap::point3d_list 55 | (e.g. all occupied nodes from getOccupied()) to 56 | * sensor_msgs::msg::PointCloud2 57 | * 58 | * @param points 59 | * @param cloud 60 | */ 61 | void pointsOctomapToPointCloud2(const point3d_list& points, 62 | sensor_msgs::msg::PointCloud2& cloud); 63 | 64 | /** 65 | * @brief Conversion from a sensor_msgs::msg::PointCLoud2 to 66 | octomap::Pointcloud, used internally in OctoMap 67 | * 68 | * @param cloud 69 | * @param octomapCloud 70 | */ 71 | 72 | void pointCloud2ToOctomap(const sensor_msgs::msg::PointCloud2& cloud, 73 | Pointcloud& octomapCloud); 74 | 75 | /// Conversion from octomap::point3d to geometry_msgs::msg::Point 76 | static inline geometry_msgs::msg::Point pointOctomapToMsg( 77 | const point3d& octomapPt){ 78 | geometry_msgs::msg::Point pt; 79 | pt.x = octomapPt.x(); 80 | pt.y = octomapPt.y(); 81 | pt.z = octomapPt.z(); 82 | 83 | return pt; 84 | } 85 | 86 | /// Conversion from geometry_msgs::msg::Point to octomap::point3d 87 | static inline octomap::point3d pointMsgToOctomap( 88 | const geometry_msgs::msg::Point& ptMsg){ 89 | return octomap::point3d(ptMsg.x, ptMsg.y, ptMsg.z); 90 | } 91 | 92 | /// Conversion from octomap::point3d to tf2::Point 93 | static inline geometry_msgs::msg::Point pointOctomapToTf( 94 | const point3d &octomapPt) { 95 | geometry_msgs::msg::Point pt; 96 | pt.x = octomapPt.x(); 97 | pt.y = octomapPt.y(); 98 | pt.z = octomapPt.z(); 99 | return pt; 100 | } 101 | 102 | /// Conversion from tf2::Point to octomap::point3d 103 | static inline octomap::point3d pointTfToOctomap( 104 | const geometry_msgs::msg::Point& ptTf){ 105 | return point3d(ptTf.x, ptTf.y, ptTf.z); 106 | } 107 | 108 | static inline octomap::point3d pointTfToOctomap( 109 | const geometry_msgs::msg::Vector3& ptTf){ 110 | return point3d(ptTf.x, ptTf.y, ptTf.z); 111 | } 112 | 113 | /// Conversion from octomap Quaternion to tf2::Quaternion 114 | static inline tf2::Quaternion quaternionOctomapToTf( 115 | const octomath::Quaternion& octomapQ){ 116 | return tf2::Quaternion( 117 | octomapQ.x(), octomapQ.y(), octomapQ.z(), octomapQ.u()); 118 | } 119 | 120 | /// Conversion from tf2::Quaternion to octomap Quaternion 121 | static inline octomath::Quaternion quaternionTfToOctomap( 122 | const tf2::Quaternion& qTf){ 123 | return octomath::Quaternion(qTf.w(), qTf.x(), qTf.y(), qTf.z()); 124 | } 125 | 126 | static inline octomath::Quaternion quaternionTfToOctomap( 127 | const geometry_msgs::msg::Quaternion &qTf){ 128 | return octomath::Quaternion(qTf.w, qTf.x, qTf.y, qTf.z); 129 | } 130 | 131 | /// Conversion from octomap::pose6f to tf2::Pose 132 | static inline geometry_msgs::msg::Pose poseOctomapToTf( 133 | const octomap::pose6d& octomapPose){ 134 | auto r = quaternionOctomapToTf(octomapPose.rot()); 135 | geometry_msgs::msg::Quaternion orientation; 136 | orientation.x = r.x(); 137 | orientation.y = r.y(); 138 | orientation.z = r.z(); 139 | orientation.w = r.w(); 140 | 141 | geometry_msgs::msg::Pose pose; 142 | pose.position = pointOctomapToTf(octomapPose.trans()); 143 | pose.orientation = orientation; 144 | return pose; 145 | } 146 | 147 | /// Conversion from tf2::Pose to octomap::pose6d 148 | static inline octomap::pose6d poseTfToOctomap( 149 | const geometry_msgs::msg::Pose& poseTf) { 150 | return octomap::pose6d(pointTfToOctomap(poseTf.position), 151 | quaternionTfToOctomap(poseTf.orientation)); 152 | } 153 | } 154 | 155 | #endif 156 | -------------------------------------------------------------------------------- /include/octomap_server2/octomap_server.hpp: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | #ifndef _OCTOMAP_SERVER_HPP_ 4 | #define _OCTOMAP_SERVER_HPP_ 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | #include 49 | 50 | #include 51 | #include 52 | 53 | #ifdef COLOR_OCTOMAP_SERVER 54 | #include 55 | #endif 56 | 57 | #ifdef COLOR_OCTOMAP_SERVER 58 | using PCLPoint = pcl::PointXYZRGB; 59 | using PCLPointCloud = pcl::PointCloud; 60 | using OcTreeT = octomap::ColorOcTree; 61 | #else 62 | using PCLPoint = pcl::PointXYZ; 63 | using PCLPointCloud = pcl::PointCloud; 64 | using OcTreeT = octomap::OcTree; 65 | #endif 66 | 67 | using OctomapSrv = octomap_msgs::srv::GetOctomap; 68 | using BBXSrv = octomap_msgs::srv::BoundingBoxQuery; 69 | 70 | 71 | namespace ph = std::placeholders; 72 | 73 | namespace octomap_server { 74 | class OctomapServer: public rclcpp::Node { 75 | 76 | protected: 77 | 78 | std::shared_ptr> m_pointCloudSub; 80 | std::shared_ptr> m_tfPointCloudSub; 82 | 83 | static std_msgs::msg::ColorRGBA heightMapColor(double h); 84 | 85 | rclcpp::Publisher::SharedPtr m_pointCloudPub; 87 | rclcpp::Publisher::SharedPtr m_fmarkerPub; 89 | rclcpp::Publisher::SharedPtr m_markerPub; 91 | rclcpp::Publisher::SharedPtr m_binaryMapPub; 93 | rclcpp::Publisher::SharedPtr m_fullMapPub; 95 | rclcpp::Publisher::SharedPtr m_mapPub; 97 | 98 | rclcpp::Service::SharedPtr m_octomapBinaryService; 99 | rclcpp::Service::SharedPtr m_octomapFullService; 100 | rclcpp::Service::SharedPtr m_clearBBXService; 101 | rclcpp::Service::SharedPtr m_resetService; 102 | 103 | std::shared_ptr buffer_; 104 | std::shared_ptr m_tfListener; 105 | 106 | std::shared_ptr m_octree; 107 | 108 | octomap::KeyRay m_keyRay; // temp storage for ray casting 109 | octomap::OcTreeKey m_updateBBXMin; 110 | octomap::OcTreeKey m_updateBBXMax; 111 | 112 | double m_maxRange; 113 | std::string m_worldFrameId; // the map frame 114 | std::string m_baseFrameId; // base of the robot for ground plane filtering 115 | bool m_useHeightMap; 116 | std_msgs::msg::ColorRGBA m_color; 117 | std_msgs::msg::ColorRGBA m_colorFree; 118 | double m_colorFactor; 119 | bool m_publishFreeSpace; 120 | double m_res; 121 | unsigned m_treeDepth; 122 | unsigned m_maxTreeDepth; 123 | double m_pointcloudMinX; 124 | double m_pointcloudMaxX; 125 | double m_pointcloudMinY; 126 | double m_pointcloudMaxY; 127 | double m_pointcloudMinZ; 128 | double m_pointcloudMaxZ; 129 | double m_occupancyMinZ; 130 | double m_occupancyMaxZ; 131 | double m_minSizeX; 132 | double m_minSizeY; 133 | bool m_filterSpeckles; 134 | bool m_filterGroundPlane; 135 | double m_groundFilterDistance; 136 | double m_groundFilterAngle; 137 | double m_groundFilterPlaneDistance; 138 | bool m_compressMap; 139 | 140 | // downprojected 2D map: 141 | bool m_incrementalUpdate; 142 | nav_msgs::msg::OccupancyGrid m_gridmap; 143 | bool m_publish2DMap; 144 | bool m_mapOriginChanged; 145 | octomap::OcTreeKey m_paddedMinKey; 146 | unsigned m_multires2DScale; 147 | bool m_projectCompleteMap; 148 | bool m_useColoredMap; 149 | 150 | inline static void updateMinKey(const octomap::OcTreeKey& in, 151 | octomap::OcTreeKey& min) { 152 | for (unsigned i = 0; i < 3; ++i) { 153 | min[i] = std::min(in[i], min[i]); 154 | } 155 | }; 156 | 157 | inline static void updateMaxKey(const octomap::OcTreeKey& in, 158 | octomap::OcTreeKey& max) { 159 | for (unsigned i = 0; i < 3; ++i) { 160 | max[i] = std::max(in[i], max[i]); 161 | } 162 | }; 163 | 164 | /// Test if key is within update area of map (2D, ignores height) 165 | inline bool isInUpdateBBX(const OcTreeT::iterator& it) const { 166 | // 2^(tree_depth-depth) voxels wide: 167 | unsigned voxelWidth = (1 << (m_maxTreeDepth - it.getDepth())); 168 | octomap::OcTreeKey key = it.getIndexKey(); // lower corner of voxel 169 | return (key[0] + voxelWidth >= m_updateBBXMin[0] 170 | && key[1] + voxelWidth >= m_updateBBXMin[1] 171 | && key[0] <= m_updateBBXMax[0] 172 | && key[1] <= m_updateBBXMax[1]); 173 | } 174 | 175 | inline unsigned mapIdx(int i, int j) const { 176 | return m_gridmap.info.width * j + i; 177 | } 178 | 179 | inline unsigned mapIdx(const octomap::OcTreeKey& key) const { 180 | return mapIdx((key[0] - m_paddedMinKey[0]) / m_multires2DScale, 181 | (key[1] - m_paddedMinKey[1]) / m_multires2DScale); 182 | 183 | } 184 | 185 | inline bool mapChanged(const nav_msgs::msg::MapMetaData& oldMapInfo, 186 | const nav_msgs::msg::MapMetaData& newMapInfo) { 187 | return (oldMapInfo.height != newMapInfo.height 188 | || oldMapInfo.width != newMapInfo.width 189 | || oldMapInfo.origin.position.x != newMapInfo.origin.position.x 190 | || oldMapInfo.origin.position.y != newMapInfo.origin.position.y); 191 | } 192 | 193 | void publishBinaryOctoMap(const rclcpp::Time &) const; 194 | void publishFullOctoMap(const rclcpp::Time &) const; 195 | virtual void publishAll(const rclcpp::Time &); 196 | 197 | virtual void insertScan( 198 | const geometry_msgs::msg::Vector3 &sensorOrigin, 199 | const PCLPointCloud& ground, 200 | const PCLPointCloud& nonground); 201 | 202 | void filterGroundPlane(const PCLPointCloud& pc, 203 | PCLPointCloud& ground, 204 | PCLPointCloud& nonground) const; 205 | 206 | bool isSpeckleNode(const octomap::OcTreeKey& key) const; 207 | 208 | virtual void handlePreNodeTraversal(const rclcpp::Time &); 209 | virtual void handlePostNodeTraversal(const rclcpp::Time &); 210 | virtual void handleNode(const OcTreeT::iterator& it) {}; 211 | virtual void handleNodeInBBX(const OcTreeT::iterator& it) {}; 212 | virtual void handleOccupiedNode(const OcTreeT::iterator& it); 213 | virtual void handleOccupiedNodeInBBX(const OcTreeT::iterator& it); 214 | virtual void handleFreeNode(const OcTreeT::iterator& it); 215 | virtual void handleFreeNodeInBBX(const OcTreeT::iterator& it); 216 | virtual void update2DMap(const OcTreeT::iterator&, bool); 217 | 218 | virtual void onInit(); 219 | virtual void subscribe(); 220 | 221 | void adjustMapData(nav_msgs::msg::OccupancyGrid& map, 222 | const nav_msgs::msg::MapMetaData& oldMapInfo) const; 223 | 224 | public: 225 | explicit OctomapServer( 226 | const rclcpp::NodeOptions &, 227 | const std::string = "octomap_server"); 228 | virtual ~OctomapServer(); 229 | virtual bool octomapBinarySrv( 230 | const std::shared_ptr , 231 | std::shared_ptr); 232 | virtual bool octomapFullSrv( 233 | const std::shared_ptr , 234 | std::shared_ptr); 235 | 236 | bool clearBBXSrv( 237 | const std::shared_ptr, 238 | std::shared_ptr); 239 | bool resetSrv( 240 | const std::shared_ptr, 241 | std::shared_ptr); 242 | 243 | virtual void insertCloudCallback( 244 | const sensor_msgs::msg::PointCloud2::ConstSharedPtr &); 245 | virtual bool openFile(const std::string& filename); 246 | 247 | }; 248 | } // octomap_server 249 | 250 | #endif // _OCTOMAP_SERVER_HPP_ 251 | -------------------------------------------------------------------------------- /include/octomap_server2/transforms.hpp: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | #ifndef pcl_ROS_TRANSFORMS_H_ 4 | #define pcl_ROS_TRANSFORMS_H_ 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace pcl_ros { 14 | 15 | /** \brief Obtain the transformation matrix from TF into an Eigen form 16 | * \param bt the TF transformation 17 | * \param out_mat the Eigen transformation 18 | */ 19 | void transformAsMatrix( 20 | const tf2::Transform &, Eigen::Matrix4f &); 21 | Eigen::Matrix4f transformAsMatrix( 22 | const geometry_msgs::msg::TransformStamped &); 23 | } 24 | 25 | #endif // PCL_ROS_TRANSFORMS_H_ 26 | 27 | -------------------------------------------------------------------------------- /launch/octomap_server_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from launch import LaunchDescription 4 | from launch_ros.actions import Node 5 | from launch.substitutions import LaunchConfiguration 6 | from launch.actions import DeclareLaunchArgument 7 | 8 | 9 | def generate_launch_description(): 10 | return LaunchDescription([ 11 | DeclareLaunchArgument('input_cloud_topic', default_value='/livox/lidar'), 12 | DeclareLaunchArgument('resolution', default_value='0.15'), 13 | DeclareLaunchArgument('frame_id', default_value='map'), 14 | DeclareLaunchArgument('base_frame_id', default_value='base_footprint'), 15 | DeclareLaunchArgument('height_map', default_value='True'), 16 | DeclareLaunchArgument('colored_map', default_value='True'), 17 | DeclareLaunchArgument('color_factor', default_value='0.8'), 18 | DeclareLaunchArgument('filter_ground', default_value='False'), 19 | DeclareLaunchArgument('filter_speckles', default_value='False'), 20 | DeclareLaunchArgument('ground_filter/distance', default_value='0.04'), 21 | DeclareLaunchArgument('ground_filter/angle', default_value='0.15'), 22 | DeclareLaunchArgument('ground_filter/plane_distance', default_value='0.07'), 23 | DeclareLaunchArgument('compress_map', default_value='True'), 24 | DeclareLaunchArgument('incremental_2D_projection', default_value='False'), 25 | DeclareLaunchArgument('sensor_model/max_range', default_value='-1.0'), 26 | DeclareLaunchArgument('sensor_model/hit', default_value='0.7'), 27 | DeclareLaunchArgument('sensor_model/miss', default_value='0.4'), 28 | DeclareLaunchArgument('sensor_model/min', default_value='0.12'), 29 | DeclareLaunchArgument('sensor_model/max', default_value='0.97'), 30 | DeclareLaunchArgument('color/r', default_value='0.0'), 31 | DeclareLaunchArgument('color/g', default_value='0.0'), 32 | DeclareLaunchArgument('color/b', default_value='1.0'), 33 | DeclareLaunchArgument('color/a', default_value='1.0'), 34 | DeclareLaunchArgument('color_free/r', default_value='0.0'), 35 | DeclareLaunchArgument('color_free/g', default_value='0.0'), 36 | DeclareLaunchArgument('color_free/b', default_value='1.0'), 37 | DeclareLaunchArgument('color_free/a', default_value='1.0'), 38 | DeclareLaunchArgument('publish_free_space', default_value='False'), 39 | Node( 40 | package='octomap_server2', 41 | executable='octomap_server', 42 | output='screen', 43 | remappings=[('cloud_in', LaunchConfiguration('input_cloud_topic'))], 44 | parameters=[{'resolution': LaunchConfiguration('resolution'), 45 | 'frame_id': LaunchConfiguration('frame_id'), 46 | 'base_frame_id': LaunchConfiguration('base_frame_id'), 47 | 'height_map': LaunchConfiguration('height_map'), 48 | 'colored_map': LaunchConfiguration('colored_map'), 49 | 'color_factor': LaunchConfiguration('color_factor'), 50 | 'filter_ground': LaunchConfiguration('filter_ground'), 51 | 'filter_speckles': LaunchConfiguration('filter_speckles'), 52 | 'ground_filter/distance': LaunchConfiguration('ground_filter/distance'), 53 | 'ground_filter/angle': LaunchConfiguration('ground_filter/angle'), 54 | 'ground_filter/plane_distance': LaunchConfiguration('ground_filter/plane_distance'), 55 | 'compress_map': LaunchConfiguration('compress_map'), 56 | 'incremental_2D_projection': LaunchConfiguration('incremental_2D_projection'), 57 | 'sensor_model/max_range': LaunchConfiguration('sensor_model/max_range'), 58 | 'sensor_model/hit': LaunchConfiguration('sensor_model/hit'), 59 | 'sensor_model/miss': LaunchConfiguration('sensor_model/miss'), 60 | 'sensor_model/min': LaunchConfiguration('sensor_model/min'), 61 | 'sensor_model/max': LaunchConfiguration('sensor_model/max'), 62 | 'color/r': LaunchConfiguration('color/r'), 63 | 'color/g': LaunchConfiguration('color/g'), 64 | 'color/b': LaunchConfiguration('color/b'), 65 | 'color/a': LaunchConfiguration('color/a'), 66 | 'color_free/r': LaunchConfiguration('color_free/r'), 67 | 'color_free/g': LaunchConfiguration('color_free/g'), 68 | 'color_free/b': LaunchConfiguration('color_free/b'), 69 | 'color_free/a': LaunchConfiguration('color_free/a'), 70 | 'publish_free_space': LaunchConfiguration('publish_free_space')}] 71 | ) 72 | ]) 73 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | octomap_server2 5 | 0.0.0 6 | Octomap server for Ros2 7 | Krishneel Chaudhary 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_components 14 | rcutils 15 | rmw 16 | rmw_implementation_cmake 17 | std_msgs 18 | sensor_msgs 19 | geometry_msgs 20 | nav_msgs 21 | visualization_msgs 22 | pcl_conversions 23 | pcl_msgs 24 | octomap_msgs 25 | 26 | launch_ros 27 | launch_xml 28 | rclcpp 29 | rclcpp_components 30 | rcutils 31 | rmw 32 | std_msgs 33 | sensor_msgs 34 | geometry_msgs 35 | nav_msgs 36 | visualization_msgs 37 | pcl_conversions 38 | pcl_msgs 39 | octomap_msgs 40 | 41 | 42 | ament_cmake 43 | 44 | 45 | -------------------------------------------------------------------------------- /src/conversions.cpp: -------------------------------------------------------------------------------- 1 | // $Id: $ 2 | 3 | /** 4 | * OctoMap ROS integration 5 | * 6 | * @author A. Hornung, University of Freiburg, Copyright (C) 2011. 7 | * @see http://www.ros.org/wiki/octomap_ros 8 | * License: BSD 9 | */ 10 | 11 | /* 12 | * Copyright (c) 2010, A. Hornung, University of Freiburg 13 | * All rights reserved. 14 | * 15 | * Redistribution and use in source and binary forms, with or without 16 | * modification, are permitted provided that the following conditions are met: 17 | * 18 | * * Redistributions of source code must retain the above copyright 19 | * notice, this list of conditions and the following disclaimer. 20 | * * Redistributions in binary form must reproduce the above copyright 21 | * notice, this list of conditions and the following disclaimer in the 22 | * documentation and/or other materials provided with the distribution. 23 | * * Neither the name of the University of Freiburg nor the names of its 24 | * contributors may be used to endorse or promote products derived from 25 | * this software without specific prior written permission. 26 | * 27 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 28 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 29 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 30 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 31 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 32 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 33 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 34 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 35 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 36 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | #include 41 | 42 | namespace octomap { 43 | 44 | /** 45 | * @brief Conversion from octomap::point3d_list 46 | (e.g. all occupied nodes from getOccupied()) to 47 | * sensor_msgs::PointCloud2 48 | * 49 | * @param points 50 | * @param cloud 51 | */ 52 | 53 | void pointsOctomapToPointCloud2(const point3d_list& points, 54 | sensor_msgs::msg::PointCloud2& cloud) { 55 | // make sure the channel is valid 56 | 57 | /* 58 | std::vector::const_iterator field_iter = 59 | cloud.fields.begin(), 60 | field_end = cloud.fields.end(); 61 | */ 62 | 63 | auto field_iter = cloud.fields.begin(); 64 | auto field_end = cloud.fields.end(); 65 | 66 | bool has_x, has_y, has_z; 67 | has_x = has_y = has_z = false; 68 | while (field_iter != field_end) { 69 | if ((field_iter->name == "x") || (field_iter->name == "X")) 70 | has_x = true; 71 | if ((field_iter->name == "y") || (field_iter->name == "Y")) 72 | has_y = true; 73 | if ((field_iter->name == "z") || (field_iter->name == "Z")) 74 | has_z = true; 75 | ++field_iter; 76 | } 77 | 78 | if ((!has_x) || (!has_y) || (!has_z)) 79 | throw std::runtime_error("One of the fields xyz does not exist"); 80 | 81 | /* 82 | sensor_msgs::msg::PointCloud2Modifier pcd_modifier(cloud); 83 | pcd_modifier.resize(points.size()); 84 | 85 | sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); 86 | sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); 87 | sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); 88 | 89 | for (point3d_list::const_iterator it = points.begin(); it != points.end(); 90 | ++it, ++iter_x, ++iter_y, ++iter_z) { 91 | *iter_x = it->x(); 92 | *iter_y = it->y(); 93 | *iter_z = it->z(); 94 | } 95 | */ 96 | } 97 | 98 | /** 99 | * @brief Conversion from a sensor_msgs::PointCLoud2 to 100 | * octomap::Pointcloud, 101 | used internally in OctoMap 102 | * 103 | * @param cloud 104 | * @param octomapCloud 105 | */ 106 | 107 | void pointCloud2ToOctomap(const sensor_msgs::msg::PointCloud2 &cloud, 108 | Pointcloud &octomapCloud){ 109 | octomapCloud.reserve(cloud.data.size() / cloud.point_step); 110 | 111 | /* 112 | sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); 113 | sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); 114 | sensor_msgs::PointCloud2ConstIterator iter_z(cloud, "z"); 115 | 116 | for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z){ 117 | // Check if the point is invalid 118 | if (!std::isnan (*iter_x) && !std::isnan (*iter_y) && !std::isnan (*iter_z)) 119 | octomapCloud.push_back(*iter_x, *iter_y, *iter_z); 120 | } 121 | */ 122 | } 123 | } 124 | 125 | 126 | -------------------------------------------------------------------------------- /src/octomap_server.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | namespace octomap_server { 5 | OctomapServer::OctomapServer( 6 | const rclcpp::NodeOptions &options, 7 | const std::string node_name): 8 | Node(node_name, options), 9 | m_octree(NULL), 10 | m_maxRange(20), 11 | m_worldFrameId("/map"), 12 | m_baseFrameId("base_footprint"), 13 | m_useHeightMap(true), 14 | m_useColoredMap(false), 15 | m_colorFactor(0.8), 16 | m_publishFreeSpace(false), 17 | m_res(0.05), 18 | m_treeDepth(0), 19 | m_maxTreeDepth(0), 20 | m_pointcloudMinX(-std::numeric_limits::max()), 21 | m_pointcloudMaxX(std::numeric_limits::max()), 22 | m_pointcloudMinY(-std::numeric_limits::max()), 23 | m_pointcloudMaxY(std::numeric_limits::max()), 24 | m_pointcloudMinZ(-std::numeric_limits::max()), 25 | m_pointcloudMaxZ(std::numeric_limits::max()), 26 | m_occupancyMinZ(-std::numeric_limits::max()), 27 | m_occupancyMaxZ(std::numeric_limits::max()), 28 | m_minSizeX(0.0), 29 | m_minSizeY(0.0), 30 | m_filterSpeckles(false), 31 | m_filterGroundPlane(false), 32 | m_groundFilterDistance(0.04), 33 | m_groundFilterAngle(0.15), 34 | m_groundFilterPlaneDistance(0.07), 35 | m_compressMap(true), 36 | m_incrementalUpdate(false) { 37 | 38 | rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); 39 | this->buffer_ = std::make_shared(clock); 40 | this->buffer_->setUsingDedicatedThread(true); 41 | this->m_tfListener = std::make_shared( 42 | *buffer_, this, false); 43 | 44 | m_worldFrameId = this->declare_parameter("frame_id", m_worldFrameId); 45 | m_baseFrameId = this->declare_parameter("base_frame_id", m_baseFrameId); 46 | m_useHeightMap = this->declare_parameter("height_map", m_useHeightMap); 47 | m_useColoredMap = this->declare_parameter("colored_map", m_useColoredMap); 48 | m_colorFactor = this->declare_parameter("color_factor", m_colorFactor); 49 | 50 | m_pointcloudMinX = this->declare_parameter( 51 | "pointcloud_min_x", m_pointcloudMinX); 52 | m_pointcloudMaxX = this->declare_parameter( 53 | "pointcloud_max_x", m_pointcloudMaxX); 54 | m_pointcloudMinY = this->declare_parameter( 55 | "pointcloud_min_y", m_pointcloudMinY); 56 | m_pointcloudMaxY = this->declare_parameter( 57 | "pointcloud_max_y", m_pointcloudMaxY); 58 | m_pointcloudMinZ = this->declare_parameter( 59 | "pointcloud_min_z", m_pointcloudMinZ); 60 | m_pointcloudMaxZ = this->declare_parameter( 61 | "pointcloud_max_z", m_pointcloudMaxZ); 62 | m_occupancyMinZ = this->declare_parameter( 63 | "occupancy_min_z", m_occupancyMinZ); 64 | m_occupancyMaxZ = this->declare_parameter( 65 | "occupancy_max_z", m_occupancyMaxZ); 66 | m_minSizeX = this->declare_parameter( 67 | "min_x_size", m_minSizeX); 68 | m_minSizeY = this->declare_parameter( 69 | "min_y_size", m_minSizeY); 70 | 71 | m_filterSpeckles = this->declare_parameter( 72 | "filter_speckles", m_filterSpeckles); 73 | m_filterGroundPlane = this->declare_parameter( 74 | "filter_ground", m_filterGroundPlane); 75 | // distance of points from plane for RANSAC 76 | m_groundFilterDistance = this->declare_parameter( 77 | "ground_filter/distance", m_groundFilterDistance); 78 | // angular derivation of found plane: 79 | m_groundFilterAngle = this->declare_parameter( 80 | "ground_filter/angle", m_groundFilterAngle); 81 | m_groundFilterPlaneDistance = this->declare_parameter( 82 | "ground_filter/plane_distance", m_groundFilterPlaneDistance); 83 | 84 | m_maxRange = this->declare_parameter( 85 | "sensor_model/max_range", m_maxRange); 86 | 87 | m_res = this->declare_parameter("resolution", m_res); 88 | double probHit = this->declare_parameter("sensor_model/hit", 0.7); 89 | double probMiss = this->declare_parameter("sensor_model/miss", 0.4); 90 | double thresMin = this->declare_parameter("sensor_model/min", 0.12); 91 | double thresMax = this->declare_parameter("sensor_model/max", 0.97); 92 | m_compressMap = this->declare_parameter("compress_map", m_compressMap); 93 | m_incrementalUpdate = this->declare_parameter( 94 | "incremental_2D_projection", false); 95 | 96 | if (m_filterGroundPlane && 97 | (m_pointcloudMinZ > 0.0 || m_pointcloudMaxZ < 0.0)) { 98 | std::string msg = "You enabled ground filtering but incoming pointclouds " + 99 | std::string("will be pre-filtered in [%ld, %ld], excluding the") + 100 | std::string("ground level z=0 This will not work."); 101 | RCLCPP_WARN(this->get_logger(), msg.c_str(), m_pointcloudMinZ, m_pointcloudMaxZ); 102 | } 103 | 104 | if (m_useHeightMap && m_useColoredMap) { 105 | std::string msg = std::string("You enabled both height map and RGB") + 106 | "color registration. This is contradictory. Defaulting to height map."; 107 | RCLCPP_WARN(this->get_logger(), msg.c_str()); 108 | m_useColoredMap = false; 109 | } 110 | 111 | if (m_useColoredMap) { 112 | #ifdef COLOR_OCTOMAP_SERVER 113 | RCLCPP_WARN( 114 | this->get_logger(), 115 | "Using RGB color registration (if information available)"); 116 | #else 117 | std::string msg = std::string("Colored map requested in launch file") + 118 | " - node not running/compiled to support colors, " + 119 | "please define COLOR_OCTOMAP_SERVER and recompile or launch " + 120 | "the octomap_color_server node"; 121 | RCLCPP_WARN(this->get_logger(), msg.c_str()); 122 | #endif 123 | } 124 | 125 | // initialize octomap object & params 126 | m_octree = std::make_shared(m_res); 127 | m_octree->setProbHit(probHit); 128 | m_octree->setProbMiss(probMiss); 129 | m_octree->setClampingThresMin(thresMin); 130 | m_octree->setClampingThresMax(thresMax); 131 | m_treeDepth = m_octree->getTreeDepth(); 132 | m_maxTreeDepth = m_treeDepth; 133 | m_gridmap.info.resolution = m_res; 134 | 135 | double r = this->declare_parameter("color/r", 0.0); 136 | double g = this->declare_parameter("color/g", 0.0); 137 | double b = this->declare_parameter("color/b", 1.0); 138 | double a = this->declare_parameter("color/a", 1.0); 139 | m_color.r = r; 140 | m_color.g = g; 141 | m_color.b = b; 142 | m_color.a = a; 143 | 144 | r = this->declare_parameter("color_free/r", 0.0); 145 | g = this->declare_parameter("color_free/g", 1.0); 146 | b = this->declare_parameter("color_free/b", 0.0); 147 | a = this->declare_parameter("color_free/a", 1.0); 148 | m_colorFree.r = r; 149 | m_colorFree.g = g; 150 | m_colorFree.b = b; 151 | m_colorFree.a = a; 152 | 153 | m_publishFreeSpace = this->declare_parameter( 154 | "publish_free_space", m_publishFreeSpace); 155 | std::string msg = std::string("Publishing non-latched (topics are only)") + 156 | "prepared as needed, will only be re-published on map change"; 157 | RCLCPP_INFO(this->get_logger(), msg.c_str()); 158 | 159 | RCLCPP_INFO(this->get_logger(), "Frame Id %s", m_worldFrameId.c_str()); 160 | RCLCPP_INFO(this->get_logger(), "Resolution %.2f", m_res); 161 | 162 | this->onInit(); 163 | } 164 | 165 | OctomapServer::~OctomapServer() { 166 | 167 | } 168 | 169 | void OctomapServer::onInit() { 170 | this->subscribe(); 171 | 172 | rclcpp::QoS qos(rclcpp::KeepLast(3)); 173 | this->m_markerPub = this->create_publisher< 174 | visualization_msgs::msg::MarkerArray>( 175 | "occupied_cells_vis_array", qos); 176 | this->m_binaryMapPub = this->create_publisher< 177 | octomap_msgs::msg::Octomap>("octomap_binary", qos); 178 | this->m_fullMapPub = this->create_publisher< 179 | octomap_msgs::msg::Octomap>("octomap_full", qos); 180 | this->m_pointCloudPub = this->create_publisher< 181 | sensor_msgs::msg::PointCloud2>( 182 | "octomap_point_cloud_centers", qos); 183 | this->m_mapPub = this->create_publisher< 184 | nav_msgs::msg::OccupancyGrid>("projected_map", qos); 185 | this->m_fmarkerPub = this->create_publisher< 186 | visualization_msgs::msg::MarkerArray>( 187 | "free_cells_vis_array", qos); 188 | } 189 | 190 | void OctomapServer::subscribe() { 191 | this->m_pointCloudSub = std::make_shared< 192 | message_filters::Subscriber>( 193 | this, "cloud_in", rmw_qos_profile_sensor_data); 194 | 195 | auto create_timer_interface = std::make_shared( 196 | this->get_node_base_interface(), 197 | this->get_node_timers_interface()); 198 | this->buffer_->setCreateTimerInterface(create_timer_interface); 199 | 200 | this->m_tfPointCloudSub = std::make_shared>( 202 | *buffer_, m_worldFrameId, 5, 203 | this->get_node_logging_interface(), 204 | this->get_node_clock_interface(), 205 | std::chrono::seconds(1)); 206 | this->m_tfPointCloudSub->connectInput(*m_pointCloudSub); 207 | this->m_tfPointCloudSub->registerCallback( 208 | std::bind(&OctomapServer::insertCloudCallback, this, ph::_1)); 209 | 210 | this->m_octomapBinaryService = this->create_service( 211 | "octomap_binary", 212 | std::bind(&OctomapServer::octomapBinarySrv, this, ph::_1, ph::_2)); 213 | this->m_octomapFullService = this->create_service( 214 | "octomap_full", 215 | std::bind(&OctomapServer::octomapFullSrv, this, ph::_1, ph::_2)); 216 | this->m_clearBBXService = this->create_service( 217 | "clear_bbx", 218 | std::bind(&OctomapServer::clearBBXSrv, this, ph::_1, ph::_2)); 219 | this->m_resetService = this->create_service( 220 | "reset", std::bind(&OctomapServer::resetSrv, this, ph::_1, ph::_2)); 221 | 222 | RCLCPP_INFO(this->get_logger(), "Setup completed!"); 223 | } 224 | 225 | bool OctomapServer::openFile(const std::string &filename){ 226 | if (filename.length() <= 3) 227 | return false; 228 | 229 | std::string suffix = filename.substr(filename.length()-3, 3); 230 | if (suffix== ".bt") { 231 | if (!m_octree->readBinary(filename)) { 232 | return false; 233 | } 234 | } else if (suffix == ".ot") { 235 | auto tree = octomap::AbstractOcTree::read(filename); 236 | if (!tree){ 237 | return false; 238 | } 239 | 240 | OcTreeT *octree = dynamic_cast(tree); 241 | m_octree = std::shared_ptr(octree); 242 | 243 | if (!m_octree) { 244 | std::string msg = "Could not read OcTree in file"; 245 | RCLCPP_ERROR(this->get_logger(), msg.c_str()); 246 | return false; 247 | } 248 | } else { 249 | return false; 250 | } 251 | 252 | RCLCPP_INFO(this->get_logger(), 253 | "Octomap file %s loaded (%zu nodes).", 254 | filename.c_str(), m_octree->size()); 255 | 256 | m_treeDepth = m_octree->getTreeDepth(); 257 | m_maxTreeDepth = m_treeDepth; 258 | m_res = m_octree->getResolution(); 259 | m_gridmap.info.resolution = m_res; 260 | double minX, minY, minZ; 261 | double maxX, maxY, maxZ; 262 | m_octree->getMetricMin(minX, minY, minZ); 263 | m_octree->getMetricMax(maxX, maxY, maxZ); 264 | 265 | m_updateBBXMin[0] = m_octree->coordToKey(minX); 266 | m_updateBBXMin[1] = m_octree->coordToKey(minY); 267 | m_updateBBXMin[2] = m_octree->coordToKey(minZ); 268 | 269 | m_updateBBXMax[0] = m_octree->coordToKey(maxX); 270 | m_updateBBXMax[1] = m_octree->coordToKey(maxY); 271 | m_updateBBXMax[2] = m_octree->coordToKey(maxZ); 272 | 273 | rclcpp::Clock::SharedPtr clock = std::make_shared(); 274 | publishAll(clock->now()); 275 | return true; 276 | } 277 | 278 | void OctomapServer::insertCloudCallback( 279 | const sensor_msgs::msg::PointCloud2::ConstSharedPtr &cloud){ 280 | auto start = std::chrono::steady_clock::now(); 281 | 282 | // 283 | // ground filtering in base frame 284 | // 285 | PCLPointCloud pc; // input cloud for filtering and ground-detection 286 | pcl::fromROSMsg(*cloud, pc); 287 | 288 | Eigen::Matrix4f sensorToWorld; 289 | geometry_msgs::msg::TransformStamped sensorToWorldTf; 290 | try { 291 | if (!this->buffer_->canTransform( 292 | m_worldFrameId, cloud->header.frame_id, 293 | cloud->header.stamp)) { 294 | throw "Failed"; 295 | } 296 | 297 | // RCLCPP_INFO(this->get_logger(), "Can transform"); 298 | 299 | sensorToWorldTf = this->buffer_->lookupTransform( 300 | m_worldFrameId, cloud->header.frame_id, 301 | cloud->header.stamp); 302 | sensorToWorld = pcl_ros::transformAsMatrix(sensorToWorldTf); 303 | } catch (tf2::TransformException &ex) { 304 | RCLCPP_WARN(this->get_logger(), "%s",ex.what()); 305 | return; 306 | } 307 | 308 | // set up filter for height range, also removes NANs: 309 | pcl::PassThrough pass_x; 310 | pass_x.setFilterFieldName("x"); 311 | pass_x.setFilterLimits(m_pointcloudMinX, m_pointcloudMaxX); 312 | pcl::PassThrough pass_y; 313 | pass_y.setFilterFieldName("y"); 314 | pass_y.setFilterLimits(m_pointcloudMinY, m_pointcloudMaxY); 315 | pcl::PassThrough pass_z; 316 | pass_z.setFilterFieldName("z"); 317 | pass_z.setFilterLimits(m_pointcloudMinZ, m_pointcloudMaxZ); 318 | 319 | PCLPointCloud pc_ground; // segmented ground plane 320 | PCLPointCloud pc_nonground; // everything else 321 | 322 | if (m_filterGroundPlane) { 323 | geometry_msgs::msg::TransformStamped baseToWorldTf; 324 | geometry_msgs::msg::TransformStamped sensorToBaseTf; 325 | 326 | try { 327 | if (!this->buffer_->canTransform( 328 | m_baseFrameId, cloud->header.frame_id, 329 | cloud->header.stamp)) { 330 | throw "Failed"; 331 | } 332 | 333 | sensorToBaseTf = this->buffer_->lookupTransform( 334 | m_baseFrameId, cloud->header.frame_id, 335 | cloud->header.stamp); 336 | baseToWorldTf = this->buffer_->lookupTransform( 337 | m_worldFrameId, m_baseFrameId, cloud->header.stamp); 338 | } catch (tf2::TransformException& ex) { 339 | std::string msg = std::string("Transform error for ground plane filter") + 340 | "You need to set the base_frame_id or disable filter_ground."; 341 | RCLCPP_ERROR(this->get_logger(), "%s %", msg, ex.what()); 342 | return; 343 | } 344 | 345 | Eigen::Matrix4f sensorToBase = 346 | pcl_ros::transformAsMatrix(sensorToBaseTf); 347 | Eigen::Matrix4f baseToWorld = 348 | pcl_ros::transformAsMatrix(baseToWorldTf); 349 | 350 | // transform pointcloud from sensor frame to fixed robot frame 351 | pcl::transformPointCloud(pc, pc, sensorToBase); 352 | pass_x.setInputCloud(pc.makeShared()); 353 | pass_x.filter(pc); 354 | pass_y.setInputCloud(pc.makeShared()); 355 | pass_y.filter(pc); 356 | pass_z.setInputCloud(pc.makeShared()); 357 | pass_z.filter(pc); 358 | filterGroundPlane(pc, pc_ground, pc_nonground); 359 | 360 | // transform clouds to world frame for insertion 361 | pcl::transformPointCloud(pc_ground, pc_ground, baseToWorld); 362 | pcl::transformPointCloud(pc_nonground, pc_nonground, baseToWorld); 363 | } else { 364 | // directly transform to map frame: 365 | pcl::transformPointCloud(pc, pc, sensorToWorld); 366 | 367 | // just filter height range: 368 | pass_x.setInputCloud(pc.makeShared()); 369 | pass_x.filter(pc); 370 | pass_y.setInputCloud(pc.makeShared()); 371 | pass_y.filter(pc); 372 | pass_z.setInputCloud(pc.makeShared()); 373 | pass_z.filter(pc); 374 | 375 | pc_nonground = pc; 376 | // pc_nonground is empty without ground segmentation 377 | pc_ground.header = pc.header; 378 | pc_nonground.header = pc.header; 379 | } 380 | 381 | insertScan(sensorToWorldTf.transform.translation, 382 | pc_ground, pc_nonground); 383 | 384 | auto end = std::chrono::steady_clock::now(); 385 | std::chrono::duration elapsed_seconds = end - start; 386 | RCLCPP_INFO(this->get_logger(), "Time lapse %f", elapsed_seconds.count()); 387 | 388 | publishAll(cloud->header.stamp); 389 | } 390 | 391 | void OctomapServer::insertScan( 392 | const geometry_msgs::msg::Vector3 &sensorOriginTf, 393 | const PCLPointCloud& ground, 394 | const PCLPointCloud& nonground) { 395 | octomap::point3d sensorOrigin = octomap::pointTfToOctomap(sensorOriginTf); 396 | 397 | if (!m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMin) 398 | || !m_octree->coordToKeyChecked(sensorOrigin, m_updateBBXMax)) { 399 | 400 | RCLCPP_WARN(this->get_logger(), 401 | "Could not generate Key for origin"); 402 | } 403 | 404 | #ifdef COLOR_OCTOMAP_SERVER 405 | unsigned char* colors = new unsigned char[3]; 406 | #endif 407 | 408 | // instead of direct scan insertion, compute update to filter ground: 409 | octomap::KeySet free_cells, occupied_cells; 410 | // insert ground points only as free: 411 | for (auto it = ground.begin(); it != ground.end(); ++it) { 412 | octomap::point3d point(it->x, it->y, it->z); 413 | // maxrange check 414 | if ((m_maxRange > 0.0) && ((point - sensorOrigin).norm() > m_maxRange) ) { 415 | point = sensorOrigin + (point - sensorOrigin).normalized() * m_maxRange; 416 | } 417 | 418 | // only clear space (ground points) 419 | if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)) { 420 | free_cells.insert(m_keyRay.begin(), m_keyRay.end()); 421 | } 422 | 423 | octomap::OcTreeKey endKey; 424 | if (m_octree->coordToKeyChecked(point, endKey)) { 425 | updateMinKey(endKey, m_updateBBXMin); 426 | updateMaxKey(endKey, m_updateBBXMax); 427 | } else { 428 | RCLCPP_ERROR(this->get_logger(), 429 | "Could not generate Key for endpoint"); 430 | } 431 | } 432 | 433 | auto start = std::chrono::steady_clock::now(); 434 | 435 | // all other points: free on ray, occupied on endpoint: 436 | for (auto it = nonground.begin(); it != nonground.end(); ++it) { 437 | octomap::point3d point(it->x, it->y, it->z); 438 | // maxrange check 439 | if ((m_maxRange < 0.0) || ((point - sensorOrigin).norm() <= m_maxRange)) { 440 | // free cells 441 | if (m_octree->computeRayKeys(sensorOrigin, point, m_keyRay)) { 442 | free_cells.insert(m_keyRay.begin(), m_keyRay.end()); 443 | } 444 | 445 | // occupied endpoint 446 | octomap::OcTreeKey key; 447 | if (m_octree->coordToKeyChecked(point, key)) { 448 | occupied_cells.insert(key); 449 | 450 | updateMinKey(key, m_updateBBXMin); 451 | updateMaxKey(key, m_updateBBXMax); 452 | 453 | #ifdef COLOR_OCTOMAP_SERVER 454 | // NB: Only read and interpret color if it's an occupied node 455 | m_octree->averageNodeColor(it->x, it->y, it->z, 456 | it->r, it->g, it->b); 457 | #endif 458 | } 459 | } else { 460 | // ray longer than maxrange:; 461 | octomap::point3d new_end = sensorOrigin + 462 | (point - sensorOrigin).normalized() * m_maxRange; 463 | if (m_octree->computeRayKeys(sensorOrigin, new_end, m_keyRay)) { 464 | free_cells.insert(m_keyRay.begin(), m_keyRay.end()); 465 | octomap::OcTreeKey endKey; 466 | if (m_octree->coordToKeyChecked(new_end, endKey)) { 467 | free_cells.insert(endKey); 468 | updateMinKey(endKey, m_updateBBXMin); 469 | updateMaxKey(endKey, m_updateBBXMax); 470 | } else { 471 | RCLCPP_ERROR(this->get_logger(), 472 | "Could not generate Key for endpoint"); 473 | } 474 | } 475 | } 476 | } 477 | 478 | auto end = std::chrono::steady_clock::now(); 479 | std::chrono::duration elapsed_seconds = end - start; 480 | RCLCPP_INFO(this->get_logger(), "Time lapse[insert] %f", elapsed_seconds.count()); 481 | 482 | // mark free cells only if not seen occupied in this cloud 483 | for(auto it = free_cells.begin(), end=free_cells.end(); 484 | it!= end; ++it){ 485 | if (occupied_cells.find(*it) == occupied_cells.end()){ 486 | m_octree->updateNode(*it, false); 487 | } 488 | } 489 | 490 | // now mark all occupied cells: 491 | for (auto it = occupied_cells.begin(), 492 | end=occupied_cells.end(); it!= end; it++) { 493 | m_octree->updateNode(*it, true); 494 | } 495 | 496 | // TODO: eval lazy+updateInner vs. proper insertion 497 | // non-lazy by default (updateInnerOccupancy() too slow for large maps) 498 | //m_octree->updateInnerOccupancy(); 499 | octomap::point3d minPt, maxPt; 500 | /* todo 501 | ROS_DEBUG_STREAM("Bounding box keys (before): " 502 | << m_updateBBXMin[0] << " " 503 | << m_updateBBXMin[1] << " " 504 | << m_updateBBXMin[2] << " / " 505 | <keyToCoord(m_updateBBXMin); 514 | maxPt = m_octree->keyToCoord(m_updateBBXMax); 515 | /* todo 516 | ROS_DEBUG_STREAM("Updated area bounding box: "<< minPt << " - "<prune(); 526 | } 527 | 528 | #ifdef COLOR_OCTOMAP_SERVER 529 | if (colors) { 530 | delete[] colors; 531 | colors = NULL; 532 | } 533 | #endif 534 | } 535 | 536 | void OctomapServer::publishAll( 537 | const rclcpp::Time &rostime) { 538 | 539 | // ros::WallTime startTime = ros::WallTime::now(); 540 | 541 | size_t octomap_size = m_octree->size(); 542 | // TODO: estimate num occ. voxels for size of arrays (reserve) 543 | if (octomap_size <= 1) { 544 | RCLCPP_WARN( 545 | this->get_logger(), 546 | "Nothing to publish, octree is empty"); 547 | return; 548 | } 549 | 550 | bool publishFreeMarkerArray = m_publishFreeSpace && 551 | m_fmarkerPub->get_subscription_count() > 0; 552 | bool publishMarkerArray = m_markerPub->get_subscription_count() > 0; 553 | bool publishPointCloud = m_pointCloudPub->get_subscription_count() > 0; 554 | bool publishBinaryMap = m_binaryMapPub->get_subscription_count() > 0; 555 | bool publishFullMap = m_fullMapPub->get_subscription_count() > 0; 556 | m_publish2DMap = m_mapPub->get_subscription_count() > 0; 557 | 558 | // init markers for free space: 559 | visualization_msgs::msg::MarkerArray freeNodesVis; 560 | // each array stores all cubes of a different size, one for each depth level: 561 | freeNodesVis.markers.resize(m_treeDepth+1); 562 | 563 | tf2::Quaternion quaternion; 564 | quaternion.setRPY(0, 0, 0.0); 565 | geometry_msgs::msg::Pose pose; 566 | pose.orientation = tf2::toMsg(quaternion); 567 | 568 | // init markers: 569 | visualization_msgs::msg::MarkerArray occupiedNodesVis; 570 | // each array stores all cubes of a different size, one for each depth level: 571 | occupiedNodesVis.markers.resize(m_treeDepth+1); 572 | 573 | // init pointcloud: 574 | pcl::PointCloud pclCloud; 575 | 576 | // call pre-traversal hook: 577 | handlePreNodeTraversal(rostime); 578 | 579 | // now, traverse all leafs in the tree: 580 | for (auto it = m_octree->begin(m_maxTreeDepth), 581 | end = m_octree->end(); it != end; ++it) { 582 | bool inUpdateBBX = isInUpdateBBX(it); 583 | 584 | // call general hook: 585 | handleNode(it); 586 | if (inUpdateBBX) { 587 | handleNodeInBBX(it); 588 | } 589 | 590 | if (m_octree->isNodeOccupied(*it)) { 591 | double z = it.getZ(); 592 | double half_size = it.getSize() / 2.0; 593 | if (z + half_size > m_occupancyMinZ && 594 | z - half_size < m_occupancyMaxZ) { 595 | double size = it.getSize(); 596 | double x = it.getX(); 597 | double y = it.getY(); 598 | #ifdef COLOR_OCTOMAP_SERVER 599 | int r = it->getColor().r; 600 | int g = it->getColor().g; 601 | int b = it->getColor().b; 602 | #endif 603 | 604 | // Ignore speckles in the map: 605 | if (m_filterSpeckles && (it.getDepth() == m_treeDepth +1) && 606 | isSpeckleNode(it.getKey())){ 607 | RCLCPP_INFO(this->get_logger(), 608 | "Ignoring single speckle at (%f,%f,%f)", x, y, z); 609 | continue; 610 | } // else: current octree node is no speckle, send it out 611 | 612 | 613 | handleOccupiedNode(it); 614 | if (inUpdateBBX) { 615 | handleOccupiedNodeInBBX(it); 616 | } 617 | 618 | //create marker: 619 | if (publishMarkerArray){ 620 | unsigned idx = it.getDepth(); 621 | assert(idx < occupiedNodesVis.markers.size()); 622 | 623 | geometry_msgs::msg::Point cubeCenter; 624 | cubeCenter.x = x; 625 | cubeCenter.y = y; 626 | cubeCenter.z = z; 627 | 628 | occupiedNodesVis.markers[idx].points.push_back(cubeCenter); 629 | if (m_useHeightMap){ 630 | double minX, minY, minZ, maxX, maxY, maxZ; 631 | m_octree->getMetricMin(minX, minY, minZ); 632 | m_octree->getMetricMax(maxX, maxY, maxZ); 633 | 634 | double h = (1.0 - std::min(std::max((cubeCenter.z-minZ)/ (maxZ - minZ), 0.0), 1.0)) *m_colorFactor; 635 | occupiedNodesVis.markers[idx].colors.push_back(heightMapColor(h)); 636 | } 637 | 638 | #ifdef COLOR_OCTOMAP_SERVER 639 | if (m_useColoredMap) { 640 | // TODO 641 | // potentially use occupancy as measure for alpha channel? 642 | std_msgs::msg::ColorRGBA _color; 643 | _color.r = (r / 255.); 644 | _color.g = (g / 255.); 645 | _color.b = (b / 255.); 646 | _color.a = 1.0; 647 | occupiedNodesVis.markers[idx].colors.push_back(_color); 648 | } 649 | #endif 650 | } 651 | 652 | // insert into pointcloud: 653 | if (publishPointCloud) { 654 | #ifdef COLOR_OCTOMAP_SERVER 655 | PCLPoint _point = PCLPoint(); 656 | _point.x = x; 657 | _point.y = y; 658 | _point.z = z; 659 | _point.r = r; 660 | _point.g = g; 661 | _point.b = b; 662 | pclCloud.push_back(_point); 663 | #else 664 | pclCloud.push_back(PCLPoint(x, y, z)); 665 | #endif 666 | } 667 | 668 | } 669 | } else { 670 | // node not occupied => mark as free in 2D map if unknown so far 671 | double z = it.getZ(); 672 | double half_size = it.getSize() / 2.0; 673 | if (z + half_size > m_occupancyMinZ && 674 | z - half_size < m_occupancyMaxZ) { 675 | handleFreeNode(it); 676 | if (inUpdateBBX) { 677 | handleFreeNodeInBBX(it); 678 | } 679 | 680 | if (m_publishFreeSpace) { 681 | double x = it.getX(); 682 | double y = it.getY(); 683 | 684 | //create marker for free space: 685 | if (publishFreeMarkerArray) { 686 | unsigned idx = it.getDepth(); 687 | assert(idx < freeNodesVis.markers.size()); 688 | 689 | geometry_msgs::msg::Point cubeCenter; 690 | cubeCenter.x = x; 691 | cubeCenter.y = y; 692 | cubeCenter.z = z; 693 | 694 | freeNodesVis.markers[idx].points.push_back(cubeCenter); 695 | } 696 | } 697 | } 698 | } 699 | } 700 | 701 | // call post-traversal hook: 702 | handlePostNodeTraversal(rostime); 703 | 704 | // finish MarkerArray: 705 | if (publishMarkerArray) { 706 | for (unsigned i= 0; i < occupiedNodesVis.markers.size(); ++i){ 707 | double size = m_octree->getNodeSize(i); 708 | 709 | occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId; 710 | occupiedNodesVis.markers[i].header.stamp = rostime; 711 | occupiedNodesVis.markers[i].ns = "map"; 712 | occupiedNodesVis.markers[i].id = i; 713 | occupiedNodesVis.markers[i].type = 714 | visualization_msgs::msg::Marker::CUBE_LIST; 715 | occupiedNodesVis.markers[i].scale.x = size; 716 | occupiedNodesVis.markers[i].scale.y = size; 717 | occupiedNodesVis.markers[i].scale.z = size; 718 | if (!m_useColoredMap) 719 | occupiedNodesVis.markers[i].color = m_color; 720 | 721 | 722 | if (occupiedNodesVis.markers[i].points.size() > 0) 723 | occupiedNodesVis.markers[i].action = 724 | visualization_msgs::msg::Marker::ADD; 725 | else 726 | occupiedNodesVis.markers[i].action = 727 | visualization_msgs::msg::Marker::DELETE; 728 | } 729 | m_markerPub->publish(occupiedNodesVis); 730 | } 731 | 732 | // finish FreeMarkerArray: 733 | if (publishFreeMarkerArray) { 734 | for (unsigned i= 0; i < freeNodesVis.markers.size(); ++i){ 735 | double size = m_octree->getNodeSize(i); 736 | 737 | freeNodesVis.markers[i].header.frame_id = m_worldFrameId; 738 | freeNodesVis.markers[i].header.stamp = rostime; 739 | freeNodesVis.markers[i].ns = "map"; 740 | freeNodesVis.markers[i].id = i; 741 | freeNodesVis.markers[i].type = 742 | visualization_msgs::msg::Marker::CUBE_LIST; 743 | freeNodesVis.markers[i].scale.x = size; 744 | freeNodesVis.markers[i].scale.y = size; 745 | freeNodesVis.markers[i].scale.z = size; 746 | freeNodesVis.markers[i].color = m_colorFree; 747 | 748 | if (freeNodesVis.markers[i].points.size() > 0) 749 | freeNodesVis.markers[i].action = 750 | visualization_msgs::msg::Marker::ADD; 751 | else 752 | freeNodesVis.markers[i].action = 753 | visualization_msgs::msg::Marker::DELETE; 754 | } 755 | m_fmarkerPub->publish(freeNodesVis); 756 | } 757 | 758 | 759 | // finish pointcloud: 760 | if (publishPointCloud) { 761 | sensor_msgs::msg::PointCloud2 cloud; 762 | pcl::toROSMsg(pclCloud, cloud); 763 | cloud.header.frame_id = m_worldFrameId; 764 | cloud.header.stamp = rostime; 765 | m_pointCloudPub->publish(cloud); 766 | } 767 | 768 | if (publishBinaryMap) { 769 | publishBinaryOctoMap(rostime); 770 | } 771 | 772 | if (publishFullMap) { 773 | publishFullOctoMap(rostime); 774 | } 775 | 776 | /* 777 | double total_elapsed = (ros::WallTime::now() - startTime).toSec(); 778 | ROS_DEBUG("Map publishing in OctomapServer took %f sec", total_elapsed); 779 | */ 780 | } 781 | 782 | bool OctomapServer::octomapBinarySrv( 783 | const std::shared_ptr req, 784 | std::shared_ptr res) { 785 | // ros::WallTime startTime = ros::WallTime::now(); 786 | RCLCPP_INFO(this->get_logger(), 787 | "Sending binary map data on service request"); 788 | res->map.header.frame_id = m_worldFrameId; 789 | res->map.header.stamp = this->get_clock()->now(); 790 | if (!octomap_msgs::binaryMapToMsg(*m_octree, res->map)) { 791 | return false; 792 | } 793 | 794 | /* 795 | double total_elapsed = (ros::WallTime::now() - startTime).toSec(); 796 | ROS_INFO("Binary octomap sent in %f sec", total_elapsed); 797 | */ 798 | return true; 799 | } 800 | 801 | bool OctomapServer::octomapFullSrv( 802 | const std::shared_ptr req, 803 | std::shared_ptr res) { 804 | RCLCPP_INFO(this->get_logger(), 805 | "Sending full map data on service request"); 806 | res->map.header.frame_id = m_worldFrameId; 807 | res->map.header.stamp = this->get_clock()->now(); 808 | 809 | if (!octomap_msgs::fullMapToMsg(*m_octree, res->map)) { 810 | return false; 811 | } 812 | return true; 813 | } 814 | 815 | bool OctomapServer::clearBBXSrv( 816 | const std::shared_ptr req, 817 | std::shared_ptr resp) { 818 | octomap::point3d min = octomap::pointMsgToOctomap(req->min); 819 | octomap::point3d max = octomap::pointMsgToOctomap(req->max); 820 | 821 | double thresMin = m_octree->getClampingThresMin(); 822 | for(auto it = m_octree->begin_leafs_bbx(min,max), 823 | end=m_octree->end_leafs_bbx(); it!= end; ++it) { 824 | it->setLogOdds(octomap::logodds(thresMin)); 825 | } 826 | m_octree->updateInnerOccupancy(); 827 | 828 | rclcpp::Clock::SharedPtr clock = std::make_shared(); 829 | publishAll(clock->now()); 830 | 831 | return true; 832 | } 833 | 834 | bool OctomapServer::resetSrv( 835 | const std::shared_ptr req, 836 | std::shared_ptr resp) { 837 | visualization_msgs::msg::MarkerArray occupiedNodesVis; 838 | occupiedNodesVis.markers.resize(m_treeDepth + 1); 839 | rclcpp::Clock::SharedPtr clock = std::make_shared(); 840 | auto rostime = clock->now(); 841 | 842 | m_octree->clear(); 843 | // clear 2D map: 844 | m_gridmap.data.clear(); 845 | m_gridmap.info.height = 0.0; 846 | m_gridmap.info.width = 0.0; 847 | m_gridmap.info.resolution = 0.0; 848 | m_gridmap.info.origin.position.x = 0.0; 849 | m_gridmap.info.origin.position.y = 0.0; 850 | 851 | RCLCPP_INFO(this->get_logger(), "Cleared octomap"); 852 | publishAll(rostime); 853 | 854 | publishBinaryOctoMap(rostime); 855 | for (auto i = 0; i < occupiedNodesVis.markers.size(); ++i){ 856 | occupiedNodesVis.markers[i].header.frame_id = m_worldFrameId; 857 | occupiedNodesVis.markers[i].header.stamp = rostime; 858 | occupiedNodesVis.markers[i].ns = "map"; 859 | occupiedNodesVis.markers[i].id = i; 860 | occupiedNodesVis.markers[i].type = 861 | visualization_msgs::msg::Marker::CUBE_LIST; 862 | occupiedNodesVis.markers[i].action = 863 | visualization_msgs::msg::Marker::DELETE; 864 | } 865 | 866 | m_markerPub->publish(occupiedNodesVis); 867 | 868 | visualization_msgs::msg::MarkerArray freeNodesVis; 869 | freeNodesVis.markers.resize(m_treeDepth + 1); 870 | 871 | for (auto i = 0; i < freeNodesVis.markers.size(); ++i) { 872 | freeNodesVis.markers[i].header.frame_id = m_worldFrameId; 873 | freeNodesVis.markers[i].header.stamp = rostime; 874 | freeNodesVis.markers[i].ns = "map"; 875 | freeNodesVis.markers[i].id = i; 876 | freeNodesVis.markers[i].type = 877 | visualization_msgs::msg::Marker::CUBE_LIST; 878 | freeNodesVis.markers[i].action = 879 | visualization_msgs::msg::Marker::DELETE; 880 | } 881 | m_fmarkerPub->publish(freeNodesVis); 882 | return true; 883 | } 884 | 885 | void OctomapServer::publishBinaryOctoMap( 886 | const rclcpp::Time& rostime) const{ 887 | 888 | octomap_msgs::msg::Octomap map; 889 | map.header.frame_id = m_worldFrameId; 890 | map.header.stamp = rostime; 891 | 892 | if (octomap_msgs::binaryMapToMsg(*m_octree, map)) { 893 | m_binaryMapPub->publish(map); 894 | } else { 895 | RCLCPP_ERROR(this->get_logger(), 896 | "Error serializing OctoMap"); 897 | } 898 | } 899 | 900 | void OctomapServer::publishFullOctoMap( 901 | const rclcpp::Time& rostime) const{ 902 | 903 | octomap_msgs::msg::Octomap map; 904 | map.header.frame_id = m_worldFrameId; 905 | map.header.stamp = rostime; 906 | 907 | if (octomap_msgs::fullMapToMsg(*m_octree, map)) { 908 | m_fullMapPub->publish(map); 909 | } else { 910 | RCLCPP_ERROR(this->get_logger(), 911 | "Error serializing OctoMap"); 912 | } 913 | } 914 | 915 | void OctomapServer::filterGroundPlane( 916 | const PCLPointCloud& pc, 917 | PCLPointCloud& ground, 918 | PCLPointCloud& nonground) const { 919 | ground.header = pc.header; 920 | nonground.header = pc.header; 921 | 922 | if (pc.size() < 50){ 923 | RCLCPP_WARN(this->get_logger(), 924 | "Pointcloud in OctomapServer too small, skipping ground plane extraction"); 925 | nonground = pc; 926 | } else { 927 | // plane detection for ground plane removal: 928 | pcl::ModelCoefficients::Ptr coefficients( 929 | new pcl::ModelCoefficients); 930 | pcl::PointIndices::Ptr inliers( 931 | new pcl::PointIndices); 932 | 933 | // Create the segmentation object and set up: 934 | pcl::SACSegmentation seg; 935 | seg.setOptimizeCoefficients (true); 936 | // TODO: 937 | // maybe a filtering based on the surface normals might be more robust / accurate? 938 | seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); 939 | seg.setMethodType(pcl::SAC_RANSAC); 940 | seg.setMaxIterations(200); 941 | seg.setDistanceThreshold (m_groundFilterDistance); 942 | seg.setAxis(Eigen::Vector3f(0, 0, 1)); 943 | seg.setEpsAngle(m_groundFilterAngle); 944 | 945 | PCLPointCloud cloud_filtered(pc); 946 | // Create the filtering object 947 | pcl::ExtractIndices extract; 948 | bool groundPlaneFound = false; 949 | 950 | while (cloud_filtered.size() > 10 && !groundPlaneFound) { 951 | seg.setInputCloud(cloud_filtered.makeShared()); 952 | seg.segment (*inliers, *coefficients); 953 | if (inliers->indices.size () == 0) { 954 | RCLCPP_INFO(this->get_logger(), 955 | "PCL segmentation did not find any plane."); 956 | break; 957 | } 958 | 959 | extract.setInputCloud(cloud_filtered.makeShared()); 960 | extract.setIndices(inliers); 961 | 962 | if (std::abs(coefficients->values.at(3)) < m_groundFilterPlaneDistance) { 963 | RCLCPP_INFO( 964 | this->get_logger(), 965 | "Ground plane found: %zu/%zu inliers. Coeff: %f %f %f %f", 966 | inliers->indices.size(), cloud_filtered.size(), 967 | coefficients->values.at(0), coefficients->values.at(1), 968 | coefficients->values.at(2), coefficients->values.at(3)); 969 | extract.setNegative(false); 970 | extract.filter(ground); 971 | 972 | // remove ground points from full pointcloud: 973 | // workaround for PCL bug: 974 | if(inliers->indices.size() != cloud_filtered.size()) { 975 | extract.setNegative(true); 976 | PCLPointCloud cloud_out; 977 | extract.filter(cloud_out); 978 | nonground += cloud_out; 979 | cloud_filtered = cloud_out; 980 | } 981 | groundPlaneFound = true; 982 | } else { 983 | RCLCPP_INFO( 984 | this->get_logger(), 985 | "Horizontal plane (not ground) found: %zu/%zu inliers. Coeff: %f %f %f %f", 986 | inliers->indices.size(), cloud_filtered.size(), 987 | coefficients->values.at(0), coefficients->values.at(1), 988 | coefficients->values.at(2), coefficients->values.at(3)); 989 | pcl::PointCloud cloud_out; 990 | extract.setNegative (false); 991 | extract.filter(cloud_out); 992 | nonground +=cloud_out; 993 | 994 | // remove current plane from scan for next iteration: 995 | // workaround for PCL bug: 996 | if(inliers->indices.size() != cloud_filtered.size()){ 997 | extract.setNegative(true); 998 | cloud_out.points.clear(); 999 | extract.filter(cloud_out); 1000 | cloud_filtered = cloud_out; 1001 | } else{ 1002 | cloud_filtered.points.clear(); 1003 | } 1004 | } 1005 | } 1006 | // TODO: also do this if overall starting pointcloud too small? 1007 | if (!groundPlaneFound){ // no plane found or remaining points too small 1008 | RCLCPP_WARN(this->get_logger(), 1009 | "No ground plane found in scan"); 1010 | 1011 | // do a rough fitlering on height to prevent spurious obstacles 1012 | pcl::PassThrough second_pass; 1013 | second_pass.setFilterFieldName("z"); 1014 | second_pass.setFilterLimits(-m_groundFilterPlaneDistance, 1015 | m_groundFilterPlaneDistance); 1016 | second_pass.setInputCloud(pc.makeShared()); 1017 | second_pass.filter(ground); 1018 | 1019 | second_pass.setNegative(true); 1020 | second_pass.filter(nonground); 1021 | } 1022 | } 1023 | } 1024 | 1025 | void OctomapServer::handlePreNodeTraversal( 1026 | const rclcpp::Time& rostime) { 1027 | if (m_publish2DMap){ 1028 | // init projected 2D map: 1029 | m_gridmap.header.frame_id = m_worldFrameId; 1030 | m_gridmap.header.stamp = rostime; 1031 | nav_msgs::msg::MapMetaData oldMapInfo = m_gridmap.info; 1032 | 1033 | // TODO: 1034 | // move most of this stuff into c'tor and init map only once(adjust if size changes) 1035 | double minX, minY, minZ, maxX, maxY, maxZ; 1036 | m_octree->getMetricMin(minX, minY, minZ); 1037 | m_octree->getMetricMax(maxX, maxY, maxZ); 1038 | 1039 | octomap::point3d minPt(minX, minY, minZ); 1040 | octomap::point3d maxPt(maxX, maxY, maxZ); 1041 | octomap::OcTreeKey minKey = m_octree->coordToKey(minPt, m_maxTreeDepth); 1042 | octomap::OcTreeKey maxKey = m_octree->coordToKey(maxPt, m_maxTreeDepth); 1043 | 1044 | RCLCPP_INFO( 1045 | this->get_logger(), 1046 | "MinKey: %d %d %d / MaxKey: %d %d %d", 1047 | minKey[0], minKey[1], minKey[2], maxKey[0], maxKey[1], maxKey[2]); 1048 | 1049 | // add padding if requested (= new min/maxPts in x&y): 1050 | double halfPaddedX = 0.5*m_minSizeX; 1051 | double halfPaddedY = 0.5*m_minSizeY; 1052 | minX = std::min(minX, -halfPaddedX); 1053 | maxX = std::max(maxX, halfPaddedX); 1054 | minY = std::min(minY, -halfPaddedY); 1055 | maxY = std::max(maxY, halfPaddedY); 1056 | minPt = octomap::point3d(minX, minY, minZ); 1057 | maxPt = octomap::point3d(maxX, maxY, maxZ); 1058 | 1059 | octomap::OcTreeKey paddedMaxKey; 1060 | if (!m_octree->coordToKeyChecked( 1061 | minPt, m_maxTreeDepth, m_paddedMinKey)) { 1062 | RCLCPP_ERROR( 1063 | this->get_logger(), 1064 | "Could not create padded min OcTree key at %f %f %f", 1065 | minPt.x(), minPt.y(), minPt.z()); 1066 | return; 1067 | } 1068 | if (!m_octree->coordToKeyChecked( 1069 | maxPt, m_maxTreeDepth, paddedMaxKey)) { 1070 | RCLCPP_ERROR( 1071 | this->get_logger(), 1072 | "Could not create padded max OcTree key at %f %f %f", 1073 | maxPt.x(), maxPt.y(), maxPt.z()); 1074 | return; 1075 | } 1076 | 1077 | RCLCPP_INFO( 1078 | this->get_logger(), 1079 | "Padded MinKey: %d %d %d / padded MaxKey: %d %d %d", 1080 | m_paddedMinKey[0], m_paddedMinKey[1], m_paddedMinKey[2], 1081 | paddedMaxKey[0], paddedMaxKey[1], paddedMaxKey[2]); 1082 | assert(paddedMaxKey[0] >= maxKey[0] && paddedMaxKey[1] >= maxKey[1]); 1083 | 1084 | m_multires2DScale = 1 << (m_treeDepth - m_maxTreeDepth); 1085 | m_gridmap.info.width = (paddedMaxKey[0] - m_paddedMinKey[0]) / 1086 | m_multires2DScale + 1; 1087 | m_gridmap.info.height = (paddedMaxKey[1] - m_paddedMinKey[1]) / 1088 | m_multires2DScale + 1; 1089 | 1090 | int mapOriginX = minKey[0] - m_paddedMinKey[0]; 1091 | int mapOriginY = minKey[1] - m_paddedMinKey[1]; 1092 | assert(mapOriginX >= 0 && mapOriginY >= 0); 1093 | 1094 | // might not exactly be min / max of octree: 1095 | octomap::point3d origin = m_octree->keyToCoord(m_paddedMinKey, m_treeDepth); 1096 | double gridRes = m_octree->getNodeSize(m_maxTreeDepth); 1097 | m_projectCompleteMap = (!m_incrementalUpdate || 1098 | (std::abs(gridRes-m_gridmap.info.resolution) > 1e-6)); 1099 | 1100 | m_gridmap.info.resolution = gridRes; 1101 | m_gridmap.info.origin.position.x = origin.x() - gridRes*0.5; 1102 | m_gridmap.info.origin.position.y = origin.y() - gridRes*0.5; 1103 | if (m_maxTreeDepth != m_treeDepth) { 1104 | m_gridmap.info.origin.position.x -= m_res/2.0; 1105 | m_gridmap.info.origin.position.y -= m_res/2.0; 1106 | } 1107 | 1108 | // workaround for multires. projection not working properly for inner nodes: 1109 | // force re-building complete map 1110 | if (m_maxTreeDepth < m_treeDepth) { 1111 | m_projectCompleteMap = true; 1112 | } 1113 | 1114 | if(m_projectCompleteMap) { 1115 | RCLCPP_INFO(this->get_logger(), "Rebuilding complete 2D map"); 1116 | m_gridmap.data.clear(); 1117 | // init to unknown: 1118 | m_gridmap.data.resize(m_gridmap.info.width * m_gridmap.info.height, -1); 1119 | 1120 | } else { 1121 | if (mapChanged(oldMapInfo, m_gridmap.info)){ 1122 | RCLCPP_INFO( 1123 | this->get_logger(), 1124 | "2D grid map size changed to %dx%d", 1125 | m_gridmap.info.width, m_gridmap.info.height); 1126 | adjustMapData(m_gridmap, oldMapInfo); 1127 | } 1128 | nav_msgs::msg::OccupancyGrid::_data_type::iterator startIt; 1129 | auto mapUpdateBBXMinX = std::max( 1130 | 0, (int(m_updateBBXMin[0]) - int(m_paddedMinKey[0])) / 1131 | int(m_multires2DScale)); 1132 | auto mapUpdateBBXMinY = std::max( 1133 | 0, (int(m_updateBBXMin[1]) - int(m_paddedMinKey[1])) / 1134 | int(m_multires2DScale)); 1135 | auto mapUpdateBBXMaxX = std::min( 1136 | int(m_gridmap.info.width-1), 1137 | (int(m_updateBBXMax[0]) - int(m_paddedMinKey[0])) / 1138 | int(m_multires2DScale)); 1139 | auto mapUpdateBBXMaxY = std::min( 1140 | int(m_gridmap.info.height-1), 1141 | (int(m_updateBBXMax[1]) - int(m_paddedMinKey[1])) / 1142 | int(m_multires2DScale)); 1143 | 1144 | assert(mapUpdateBBXMaxX > mapUpdateBBXMinX); 1145 | assert(mapUpdateBBXMaxY > mapUpdateBBXMinY); 1146 | 1147 | auto numCols = mapUpdateBBXMaxX-mapUpdateBBXMinX +1; 1148 | 1149 | // test for max idx: 1150 | auto max_idx = m_gridmap.info.width*mapUpdateBBXMaxY + mapUpdateBBXMaxX; 1151 | if (max_idx >= m_gridmap.data.size()) { 1152 | RCLCPP_ERROR( 1153 | this->get_logger(), 1154 | std::string("BBX index not valid:").c_str(), 1155 | "%d (max index %zu for size %d x %d) update-BBX is: ", 1156 | "[%zu %zu]-[%zu %zu]", 1157 | max_idx, m_gridmap.data.size(), m_gridmap.info.width, 1158 | m_gridmap.info.height, mapUpdateBBXMinX, 1159 | mapUpdateBBXMinY, mapUpdateBBXMaxX, mapUpdateBBXMaxY); 1160 | } 1161 | 1162 | // reset proj. 2D map in bounding box: 1163 | for (unsigned int j = mapUpdateBBXMinY; j <= mapUpdateBBXMaxY; ++j) { 1164 | std::fill_n( 1165 | m_gridmap.data.begin() + m_gridmap.info.width*j+mapUpdateBBXMinX, 1166 | numCols, -1); 1167 | } 1168 | } 1169 | } 1170 | } 1171 | 1172 | void OctomapServer::handlePostNodeTraversal( 1173 | const rclcpp::Time& rostime){ 1174 | if (m_publish2DMap) { 1175 | m_mapPub->publish(m_gridmap); 1176 | } 1177 | } 1178 | 1179 | void OctomapServer::handleOccupiedNode( 1180 | const OcTreeT::iterator& it){ 1181 | if (m_publish2DMap && m_projectCompleteMap){ 1182 | update2DMap(it, true); 1183 | } 1184 | } 1185 | 1186 | void OctomapServer::handleFreeNode( 1187 | const OcTreeT::iterator& it){ 1188 | if (m_publish2DMap && m_projectCompleteMap){ 1189 | update2DMap(it, false); 1190 | } 1191 | } 1192 | 1193 | void OctomapServer::handleOccupiedNodeInBBX( 1194 | const OcTreeT::iterator& it){ 1195 | if (m_publish2DMap && !m_projectCompleteMap){ 1196 | update2DMap(it, true); 1197 | } 1198 | } 1199 | 1200 | void OctomapServer::handleFreeNodeInBBX( 1201 | const OcTreeT::iterator& it){ 1202 | if (m_publish2DMap && !m_projectCompleteMap){ 1203 | update2DMap(it, false); 1204 | } 1205 | } 1206 | 1207 | void OctomapServer::update2DMap( 1208 | const OcTreeT::iterator& it, bool occupied) { 1209 | if (it.getDepth() == m_maxTreeDepth){ 1210 | auto idx = mapIdx(it.getKey()); 1211 | if (occupied) { 1212 | m_gridmap.data[mapIdx(it.getKey())] = 100; 1213 | } else if (m_gridmap.data[idx] == -1){ 1214 | m_gridmap.data[idx] = 0; 1215 | } 1216 | } else { 1217 | int intSize = 1 << (m_maxTreeDepth - it.getDepth()); 1218 | octomap::OcTreeKey minKey=it.getIndexKey(); 1219 | for(int dx = 0; dx < intSize; dx++) { 1220 | int i = (minKey[0] + dx - m_paddedMinKey[0]) / m_multires2DScale; 1221 | for(int dy = 0; dy < intSize; dy++){ 1222 | auto idx = mapIdx(i, (minKey[1] + dy - m_paddedMinKey[1]) / 1223 | m_multires2DScale); 1224 | if (occupied) { 1225 | m_gridmap.data[idx] = 100; 1226 | } else if (m_gridmap.data[idx] == -1) { 1227 | m_gridmap.data[idx] = 0; 1228 | } 1229 | } 1230 | } 1231 | } 1232 | } 1233 | 1234 | bool OctomapServer::isSpeckleNode( 1235 | const octomap::OcTreeKey &nKey) const { 1236 | octomap::OcTreeKey key; 1237 | bool neighborFound = false; 1238 | for (key[2] = nKey[2] - 1; !neighborFound && key[2] <= nKey[2] + 1; ++key[2]) { 1239 | for (key[1] = nKey[1] - 1; !neighborFound && key[1] <= nKey[1] + 1; ++key[1]) { 1240 | for (key[0] = nKey[0] - 1; !neighborFound && key[0] <= nKey[0] + 1; ++key[0]) { 1241 | if (key != nKey) { 1242 | auto node = m_octree->search(key); 1243 | if (node && m_octree->isNodeOccupied(node)) { 1244 | neighborFound = true; 1245 | } 1246 | } 1247 | } 1248 | } 1249 | } 1250 | return neighborFound; 1251 | } 1252 | 1253 | void OctomapServer::adjustMapData( 1254 | nav_msgs::msg::OccupancyGrid &map, 1255 | const nav_msgs::msg::MapMetaData &oldMapInfo) const { 1256 | if (map.info.resolution != oldMapInfo.resolution) { 1257 | RCLCPP_ERROR(this->get_logger(), 1258 | "Resolution of map changed, cannot be adjusted"); 1259 | return; 1260 | } 1261 | 1262 | int i_off = int( 1263 | (oldMapInfo.origin.position.x - map.info.origin.position.x) / 1264 | map.info.resolution + 0.5); 1265 | int j_off = int( 1266 | (oldMapInfo.origin.position.y - map.info.origin.position.y) / 1267 | map.info.resolution + 0.5); 1268 | 1269 | if (i_off < 0 || j_off < 0 1270 | || oldMapInfo.width + i_off > map.info.width 1271 | || oldMapInfo.height + j_off > map.info.height) { 1272 | RCLCPP_ERROR( 1273 | this->get_logger(), 1274 | "New 2D map does not contain old map area, this case is not implemented"); 1275 | return; 1276 | } 1277 | 1278 | // nav_msgs::msg::OccupancyGrid::_data_type oldMapData = 1279 | // map.data; 1280 | auto oldMapData = map.data; 1281 | 1282 | map.data.clear(); 1283 | // init to unknown: 1284 | map.data.resize(map.info.width * map.info.height, -1); 1285 | nav_msgs::msg::OccupancyGrid::_data_type::iterator fromStart, fromEnd, toStart; 1286 | 1287 | for (int j = 0; j < int(oldMapInfo.height); ++j) { 1288 | // copy chunks, row by row: 1289 | fromStart = oldMapData.begin() + j*oldMapInfo.width; 1290 | fromEnd = fromStart + oldMapInfo.width; 1291 | toStart = map.data.begin() + ((j+j_off)*m_gridmap.info.width + i_off); 1292 | copy(fromStart, fromEnd, toStart); 1293 | } 1294 | } 1295 | 1296 | std_msgs::msg::ColorRGBA OctomapServer::heightMapColor(double h) { 1297 | std_msgs::msg::ColorRGBA color; 1298 | color.a = 1.0; 1299 | // blend over HSV-values (more colors) 1300 | 1301 | double s = 1.0; 1302 | double v = 1.0; 1303 | 1304 | h -= floor(h); 1305 | h *= 6; 1306 | int i; 1307 | double m, n, f; 1308 | 1309 | i = floor(h); 1310 | f = h - i; 1311 | if (!(i & 1)) 1312 | f = 1 - f; // if i is even 1313 | m = v * (1 - s); 1314 | n = v * (1 - s * f); 1315 | 1316 | switch (i) { 1317 | case 6: 1318 | case 0: 1319 | color.r = v; color.g = n; color.b = m; 1320 | break; 1321 | case 1: 1322 | color.r = n; color.g = v; color.b = m; 1323 | break; 1324 | case 2: 1325 | color.r = m; color.g = v; color.b = n; 1326 | break; 1327 | case 3: 1328 | color.r = m; color.g = n; color.b = v; 1329 | break; 1330 | case 4: 1331 | color.r = n; color.g = m; color.b = v; 1332 | break; 1333 | case 5: 1334 | color.r = v; color.g = m; color.b = n; 1335 | break; 1336 | default: 1337 | color.r = 1; color.g = 0.5; color.b = 0.5; 1338 | break; 1339 | } 1340 | return color; 1341 | } 1342 | } 1343 | 1344 | RCLCPP_COMPONENTS_REGISTER_NODE(octomap_server::OctomapServer) 1345 | -------------------------------------------------------------------------------- /src/transforms.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | namespace pcl_ros { 5 | 6 | void transformAsMatrix(const tf2::Transform &bt, 7 | Eigen::Matrix4f &out_mat) { 8 | double mv[12]; 9 | bt.getBasis ().getOpenGLSubMatrix (mv); 10 | 11 | tf2::Vector3 origin = bt.getOrigin (); 12 | 13 | out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8]; 14 | out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9]; 15 | out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10]; 16 | 17 | out_mat (3, 0) = out_mat(3, 1) = out_mat(3, 2) = 0; 18 | out_mat(3, 3) = 1; 19 | out_mat (0, 3) = origin.x(); 20 | out_mat (1, 3) = origin.y(); 21 | out_mat (2, 3) = origin.z(); 22 | } 23 | 24 | Eigen::Matrix4f transformAsMatrix( 25 | const geometry_msgs::msg::TransformStamped &transform_stamped) { 26 | 27 | auto t = transform_stamped.transform.translation; 28 | auto r = transform_stamped.transform.rotation; 29 | tf2::Quaternion quaternion(r.x, r.y, r.z, r.w); 30 | tf2::Vector3 translation(t.x, t.y, t.z); 31 | 32 | tf2::Transform transform; 33 | transform.setOrigin(translation); 34 | transform.setRotation(quaternion); 35 | 36 | Eigen::Matrix4f out_mat; 37 | transformAsMatrix(transform, out_mat); 38 | return out_mat; 39 | } 40 | } // namespace pcl_ros 41 | --------------------------------------------------------------------------------