├── .github └── workflows │ └── industrial_ci_action.yml ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include └── octomap_rviz_plugins │ ├── occupancy_grid_display.h │ └── occupancy_map_display.h ├── package.xml ├── plugin_description.xml └── src ├── occupancy_grid_display.cpp └── occupancy_map_display.cpp /.github/workflows/industrial_ci_action.yml: -------------------------------------------------------------------------------- 1 | name: ROS1-CI 2 | on: [push, pull_request] # on all pushes and PRs 3 | 4 | jobs: 5 | CI: 6 | strategy: 7 | matrix: 8 | env: 9 | - {ROS_DISTRO: melodic} 10 | - {ROS_DISTRO: noetic} 11 | - {ROS_DISTRO: melodic, PRERELEASE: true} 12 | - {ROS_DISTRO: noetic, PRERELEASE: true} 13 | env: 14 | CCACHE_DIR: /github/home/.ccache # Enable ccache 15 | runs-on: ubuntu-latest 16 | steps: 17 | - uses: actions/checkout@v3 18 | # This step will fetch/store the directory used by ccache before/after the ci run 19 | - uses: actions/cache@v3 20 | with: 21 | path: ${{ env.CCACHE_DIR }} 22 | key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }} 23 | # Run industrial_ci 24 | - uses: 'ros-industrial/industrial_ci@6a8f546cbd31fbd5c9f77e3409265c8b39abc3d6' 25 | env: ${{ matrix.env }} 26 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.project 2 | *.cproject 3 | *.*~ 4 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | 0.2.4 (2021-12-21) 2 | ------------------ 3 | * Use automoc feature of CMake (`#34 `_) 4 | * Contributors: Simon Schmeisser (isys vision), Wolfgang Merkt 5 | 6 | 0.2.3 (2021-01-17) 7 | ------------------ 8 | * Build plugin-lib as module (`#36 `_) 9 | * Add GitHub Actions CI, address warnings for ROS Noetic (`#39 `_) 10 | * Contributors: Sebastian Kasperski, Wolfgang Merkt 11 | 12 | 0.2.2 (2019-07-16) 13 | ------------------ 14 | * Add missing Qt5 dependencies `#35 `_ 15 | * Contributors: Wolfgang Merkt 16 | 17 | 0.2.1 (2019-07-11) 18 | ------------------ 19 | * Add Wolfgang Merkt to maintainers 20 | * `Update visualization on property changes with latched topics `_ 21 | * `Update node frame from TF on every frame `_ 22 | * `Fixed rendering of free-space voxels and pruning enclosed nodes `_ 23 | * Contributors: Armin Hornung, Matthias Nieuwenhuisen, Vladimir Ivan, Wolfgang Merkt 24 | 25 | 0.2.0 (2016-07-10) 26 | ------------------ 27 | * Fix: RViz uses Qt5 in kinetic 28 | * Contributors: Armin Hornung 29 | 30 | 0.1.0 (2016-07-07) 31 | ------------------ 32 | * Remove -ldefault_plugin from linker options, fixes `#19 `_ 33 | * Support for displaying ColorOcTree and OcTreeStamped, templated rviz plugins 34 | * Trim Z values in the octomap visualization 35 | * Add alpha property to OccupancyGridDisplay 36 | * add fix for qt moc BOOST_JOIN problem for osx yosemite build 37 | * Contributors: Armin Hornung, Felix Endres, Gautham Manoharan, Javier V. Gómez, Oleksandr Lavrushchenko, Ryohei Ueda 38 | 39 | 0.0.5 (2013-09-06) 40 | ------------------ 41 | * fix a crash when the destructor is called before onInitialize 42 | * Fix descriptions, limit octree depth range 43 | * Porting fixes from groovy branch (QT4_WRAP macro, OCTOMAP_INCLUDE_DIRS) 44 | 45 | 0.0.4 (2013-07-05) 46 | ------------------ 47 | * Safer checking for octree conversion from stream 48 | * Create octomap using AbstracOcTree (Fix issue #1) 49 | 50 | 0.0.3 (2013-06-26) 51 | ------------------ 52 | * correct CMakeLists. octomap_INCLUDE_DIRS(or LIB..) to OCTOMAP_INCLUDE_DIRS 53 | 54 | 0.0.2 (2013-05-08) 55 | ------------------ 56 | * 0.0.1 -> 0.0.2 57 | * adding OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED option to make QT4_WRAP macro happy 58 | 59 | 0.0.1 (2013-05-04) 60 | ------------------ 61 | * removing dependency to octomap_ros package + clean-up 62 | * removing rosbuild Makefile 63 | * major revision 64 | * adjusting to recent rviz api changes 65 | * removed pcl dependancy 66 | * fixed threading issue 67 | * working version with ogre point cloud structures and colored boxes 68 | * added max depth filter 69 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(octomap_rviz_plugins) 3 | 4 | find_package(catkin REQUIRED COMPONENTS octomap_msgs 5 | roscpp 6 | rviz 7 | 8 | ) 9 | 10 | find_package(octomap REQUIRED) 11 | find_package(Boost REQUIRED COMPONENTS thread ) 12 | 13 | find_package(Qt5 COMPONENTS Core Widgets REQUIRED) 14 | set(QT_LIBRARIES Qt5::Widgets) 15 | 16 | set(CMAKE_AUTOMOC ON) 17 | 18 | add_definitions(-DQT_NO_KEYWORDS) 19 | 20 | catkin_package( 21 | INCLUDE_DIRS include 22 | LIBRARIES ${PROJECT_NAME} 23 | CATKIN_DEPENDS octomap_msgs 24 | roscpp 25 | rviz 26 | DEPENDS OCTOMAP 27 | ) 28 | 29 | 30 | include_directories(include 31 | ${catkin_INCLUDE_DIRS} 32 | ${Boost_INCLUDE_DIRS} 33 | ${OCTOMAP_INCLUDE_DIRS}) 34 | 35 | link_directories(${catkin_LIBRARY_DIRS} 36 | ${Boost_LIBRARY_DIRS} 37 | ${OCTOMAP_LIBRARY_DIRS}) 38 | 39 | 40 | set(SOURCE_FILES 41 | src/occupancy_grid_display.cpp 42 | src/occupancy_map_display.cpp 43 | ) 44 | 45 | set(HEADER_FILES 46 | include/octomap_rviz_plugins/occupancy_grid_display.h 47 | include/octomap_rviz_plugins/occupancy_map_display.h 48 | ) 49 | 50 | add_library(${PROJECT_NAME} MODULE ${SOURCE_FILES} ${HEADER_FILES}) 51 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${Boost_LIBRARIES} ${OCTOMAP_LIBRARIES} ${catkin_LIBRARIES}) 52 | target_compile_options(${PROJECT_NAME} PUBLIC "-Wno-register") # Avoid OGRE deprecaton warnings under C++17 53 | 54 | install(DIRECTORY include/${PROJECT_NAME}/ 55 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 56 | ) 57 | 58 | install(TARGETS ${PROJECT_NAME} 59 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 62 | ) 63 | 64 | install(FILES plugin_description.xml 65 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 66 | ) 67 | 68 | 69 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | octomap_rviz_plugins [![CI](https://github.com/octomap/octomap_rviz_plugins/workflows/CI/badge.svg)](https://github.com/octomap/octomap_rviz_plugins/actions?query=workflow%3ACI) 2 | ==================== 3 | 4 | RViz display plugins for visualizing octomap messages (ROS groovy and later): 5 | http://ros.org/wiki/octomap_rviz_plugins 6 | -------------------------------------------------------------------------------- /include/octomap_rviz_plugins/occupancy_grid_display.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | * 31 | */ 32 | 33 | #ifndef RVIZ_OCCUPANCY_GRID_DISPLAY_H 34 | #define RVIZ_OCCUPANCY_GRID_DISPLAY_H 35 | 36 | #ifndef Q_MOC_RUN 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | #include 47 | #include 48 | 49 | #include 50 | #include "rviz/ogre_helpers/point_cloud.h" 51 | 52 | #endif 53 | 54 | namespace rviz { 55 | class RosTopicProperty; 56 | class IntProperty; 57 | class EnumProperty; 58 | class FloatProperty; 59 | } 60 | 61 | namespace octomap_rviz_plugin 62 | { 63 | 64 | class OccupancyGridDisplay : public rviz::Display 65 | { 66 | Q_OBJECT 67 | public: 68 | OccupancyGridDisplay(); 69 | virtual ~OccupancyGridDisplay(); 70 | 71 | // Overrides from Display 72 | virtual void onInitialize(); 73 | virtual void update(float wall_dt, float ros_dt); 74 | virtual void reset(); 75 | 76 | private Q_SLOTS: 77 | void updateQueueSize(); 78 | void updateTopic(); 79 | void updateTreeDepth(); 80 | void updateOctreeRenderMode(); 81 | void updateOctreeColorMode(); 82 | void updateAlpha(); 83 | void updateMaxHeight(); 84 | void updateMinHeight(); 85 | 86 | protected: 87 | // overrides from Display 88 | virtual void onEnable(); 89 | virtual void onDisable(); 90 | 91 | void subscribe(); 92 | void unsubscribe(); 93 | 94 | virtual void incomingMessageCallback(const octomap_msgs::OctomapConstPtr& msg) = 0; 95 | 96 | void setColor( double z_pos, double min_z, double max_z, double color_factor, rviz::PointCloud::Point& point); 97 | 98 | void clear(); 99 | 100 | virtual bool updateFromTF(); 101 | 102 | typedef std::vector VPoint; 103 | typedef std::vector VVPoint; 104 | 105 | boost::shared_ptr > sub_; 106 | 107 | boost::mutex mutex_; 108 | 109 | // point buffer 110 | VVPoint new_points_; 111 | VVPoint point_buf_; 112 | bool new_points_received_; 113 | 114 | // Ogre-rviz point clouds 115 | std::vector cloud_; 116 | std::vector box_size_; 117 | std_msgs::Header header_; 118 | 119 | // Plugin properties 120 | rviz::IntProperty* queue_size_property_; 121 | rviz::RosTopicProperty* octomap_topic_property_; 122 | rviz::EnumProperty* octree_render_property_; 123 | rviz::EnumProperty* octree_coloring_property_; 124 | rviz::IntProperty* tree_depth_property_; 125 | rviz::FloatProperty* alpha_property_; 126 | rviz::FloatProperty* max_height_property_; 127 | rviz::FloatProperty* min_height_property_; 128 | 129 | u_int32_t queue_size_; 130 | uint32_t messages_received_; 131 | double color_factor_; 132 | }; 133 | 134 | template 135 | class TemplatedOccupancyGridDisplay: public OccupancyGridDisplay { 136 | protected: 137 | void incomingMessageCallback(const octomap_msgs::OctomapConstPtr& msg); 138 | void setVoxelColor(rviz::PointCloud::Point& newPoint, typename OcTreeType::NodeType& node, double minZ, double maxZ); 139 | ///Returns false, if the type_id (of the message) does not correspond to the template paramter 140 | ///of this class, true if correct or unknown (i.e., no specialized method for that template). 141 | bool checkType(std::string type_id); 142 | }; 143 | 144 | } // namespace octomap_rviz_plugin 145 | 146 | #endif //RVIZ_OCCUPANCY_GRID_DISPLAY_H 147 | -------------------------------------------------------------------------------- /include/octomap_rviz_plugins/occupancy_map_display.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | * 31 | */ 32 | 33 | #ifndef RVIZ_OCCUPANCY_MAP_DISPLAY_H 34 | #define RVIZ_OCCUPANCY_MAP_DISPLAY_H 35 | 36 | #ifndef Q_MOC_RUN 37 | 38 | #include 39 | 40 | #include 41 | 42 | #include "rviz/default_plugin/map_display.h" 43 | 44 | #include 45 | 46 | #include 47 | 48 | #include 49 | 50 | #endif 51 | 52 | namespace octomap_rviz_plugin 53 | { 54 | 55 | class OccupancyMapDisplay: public rviz::MapDisplay 56 | { 57 | Q_OBJECT 58 | public: 59 | OccupancyMapDisplay(); 60 | virtual ~OccupancyMapDisplay(); 61 | 62 | private Q_SLOTS: 63 | void updateTopic(); 64 | void updateTreeDepth(); 65 | 66 | protected: 67 | virtual void onInitialize(); 68 | virtual void subscribe(); 69 | virtual void unsubscribe(); 70 | 71 | virtual void handleOctomapBinaryMessage(const octomap_msgs::OctomapConstPtr& msg) = 0; 72 | 73 | boost::shared_ptr > sub_; 74 | 75 | unsigned int octree_depth_; 76 | rviz::IntProperty* tree_depth_property_; 77 | }; 78 | 79 | template 80 | class TemplatedOccupancyMapDisplay: public OccupancyMapDisplay { 81 | protected: 82 | void handleOctomapBinaryMessage(const octomap_msgs::OctomapConstPtr& msg); 83 | }; 84 | 85 | } // namespace rviz 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | octomap_rviz_plugins 3 | 0.2.4 4 | 5 | A set of plugins for displaying occupancy information decoded from binary octomap messages. 6 | 7 | 8 | Armin Hornung 9 | Wolfgang Merkt 10 | 11 | http://ros.org/wiki/octomap_rviz_plugins 12 | https://github.com/OctoMap/octomap_rviz_plugins/issues 13 | 14 | BSD 15 | 16 | Julius Kammerl 17 | 18 | catkin 19 | 20 | octomap_msgs 21 | octomap 22 | roscpp 23 | rviz 24 | qtbase5-dev 25 | libqt5-core 26 | libqt5-widgets 27 | 28 | octomap_msgs 29 | octomap 30 | roscpp 31 | rviz 32 | qtbase5-dev 33 | libqt5-core 34 | libqt5-widgets 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | Displays 3D occupancy grids generated from compressed octomap messages. 8 | 9 | 10 | 13 | 14 | Displays 3D occupancy grids generated from compressed color octomap messages. 15 | 16 | 17 | 20 | 21 | Displays 3D occupancy grids generated from compressed octomap messages. 22 | 23 | 24 | 25 | 28 | 29 | Displays projected 2D occupancy maps generated from compressed octomap messages. 30 | 31 | 32 | 35 | 36 | Displays projected 2D occupancy maps generated from compressed octomap messages. 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /src/occupancy_grid_display.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | * 31 | */ 32 | #include 33 | 34 | #include "octomap_rviz_plugins/occupancy_grid_display.h" 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | #include "rviz/visualization_manager.h" 43 | #include "rviz/frame_manager.h" 44 | #include "rviz/properties/int_property.h" 45 | #include "rviz/properties/ros_topic_property.h" 46 | #include "rviz/properties/enum_property.h" 47 | #include "rviz/properties/float_property.h" 48 | 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | 55 | #include 56 | 57 | 58 | using namespace rviz; 59 | 60 | namespace octomap_rviz_plugin 61 | { 62 | 63 | static const std::size_t max_octree_depth_ = sizeof(unsigned short) * 8; 64 | 65 | enum OctreeVoxelRenderMode 66 | { 67 | OCTOMAP_FREE_VOXELS = 1, 68 | OCTOMAP_OCCUPIED_VOXELS = 2 69 | }; 70 | 71 | enum OctreeVoxelColorMode 72 | { 73 | OCTOMAP_CELL_COLOR, 74 | OCTOMAP_Z_AXIS_COLOR, 75 | OCTOMAP_PROBABLILTY_COLOR, 76 | }; 77 | 78 | OccupancyGridDisplay::OccupancyGridDisplay() : 79 | rviz::Display(), 80 | new_points_received_(false), 81 | messages_received_(0), 82 | queue_size_(5), 83 | color_factor_(0.8) 84 | { 85 | 86 | octomap_topic_property_ = new RosTopicProperty( "Octomap Topic", 87 | "", 88 | QString::fromStdString(ros::message_traits::datatype()), 89 | "octomap_msgs::Octomap topic to subscribe to (binary or full probability map)", 90 | this, 91 | SLOT( updateTopic() )); 92 | 93 | queue_size_property_ = new IntProperty( "Queue Size", 94 | queue_size_, 95 | "Advanced: set the size of the incoming message queue. Increasing this " 96 | "is useful if your incoming TF data is delayed significantly from your" 97 | " image data, but it can greatly increase memory usage if the messages are big.", 98 | this, 99 | SLOT( updateQueueSize() )); 100 | queue_size_property_->setMin(1); 101 | 102 | octree_render_property_ = new rviz::EnumProperty( "Voxel Rendering", "Occupied Voxels", 103 | "Select voxel type.", 104 | this, 105 | SLOT( updateOctreeRenderMode() ) ); 106 | 107 | octree_render_property_->addOption( "Occupied Voxels", OCTOMAP_OCCUPIED_VOXELS ); 108 | octree_render_property_->addOption( "Free Voxels", OCTOMAP_FREE_VOXELS ); 109 | octree_render_property_->addOption( "All Voxels", OCTOMAP_FREE_VOXELS | OCTOMAP_OCCUPIED_VOXELS); 110 | 111 | octree_coloring_property_ = new rviz::EnumProperty( "Voxel Coloring", "Z-Axis", 112 | "Select voxel coloring mode", 113 | this, 114 | SLOT( updateOctreeColorMode() ) ); 115 | 116 | octree_coloring_property_->addOption( "Cell Color", OCTOMAP_CELL_COLOR ); 117 | octree_coloring_property_->addOption( "Z-Axis", OCTOMAP_Z_AXIS_COLOR ); 118 | octree_coloring_property_->addOption( "Cell Probability", OCTOMAP_PROBABLILTY_COLOR ); 119 | alpha_property_ = new rviz::FloatProperty( "Voxel Alpha", 1.0, "Set voxel transparency alpha", 120 | this, 121 | SLOT( updateAlpha() ) ); 122 | alpha_property_->setMin(0.0); 123 | alpha_property_->setMax(1.0); 124 | 125 | tree_depth_property_ = new IntProperty("Max. Octree Depth", 126 | max_octree_depth_, 127 | "Defines the maximum tree depth", 128 | this, 129 | SLOT (updateTreeDepth() )); 130 | tree_depth_property_->setMin(0); 131 | 132 | max_height_property_ = new FloatProperty("Max. Height Display", 133 | std::numeric_limits::infinity(), 134 | "Defines the maximum height to display", 135 | this, 136 | SLOT (updateMaxHeight() )); 137 | 138 | min_height_property_ = new FloatProperty("Min. Height Display", 139 | -std::numeric_limits::infinity(), 140 | "Defines the minimum height to display", 141 | this, 142 | SLOT (updateMinHeight() )); 143 | } 144 | 145 | void OccupancyGridDisplay::onInitialize() 146 | { 147 | boost::mutex::scoped_lock lock(mutex_); 148 | 149 | box_size_.resize(max_octree_depth_); 150 | cloud_.resize(max_octree_depth_); 151 | point_buf_.resize(max_octree_depth_); 152 | new_points_.resize(max_octree_depth_); 153 | 154 | for (std::size_t i = 0; i < max_octree_depth_; ++i) 155 | { 156 | std::stringstream sname; 157 | sname << "PointCloud Nr." << i; 158 | cloud_[i] = new rviz::PointCloud(); 159 | cloud_[i]->setName(sname.str()); 160 | cloud_[i]->setRenderMode(rviz::PointCloud::RM_BOXES); 161 | scene_node_->attachObject(cloud_[i]); 162 | } 163 | } 164 | 165 | OccupancyGridDisplay::~OccupancyGridDisplay() 166 | { 167 | std::size_t i; 168 | 169 | unsubscribe(); 170 | 171 | for (std::vector::iterator it = cloud_.begin(); it != cloud_.end(); ++it) { 172 | delete *(it); 173 | } 174 | 175 | if (scene_node_) 176 | scene_node_->detachAllObjects(); 177 | } 178 | 179 | void OccupancyGridDisplay::updateQueueSize() 180 | { 181 | queue_size_ = queue_size_property_->getInt(); 182 | 183 | subscribe(); 184 | } 185 | 186 | void OccupancyGridDisplay::onEnable() 187 | { 188 | scene_node_->setVisible(true); 189 | subscribe(); 190 | } 191 | 192 | void OccupancyGridDisplay::onDisable() 193 | { 194 | scene_node_->setVisible(false); 195 | unsubscribe(); 196 | 197 | clear(); 198 | } 199 | 200 | void OccupancyGridDisplay::subscribe() 201 | { 202 | if (!isEnabled()) 203 | { 204 | return; 205 | } 206 | 207 | try 208 | { 209 | unsubscribe(); 210 | 211 | const std::string& topicStr = octomap_topic_property_->getStdString(); 212 | 213 | if (!topicStr.empty()) 214 | { 215 | 216 | sub_.reset(new message_filters::Subscriber()); 217 | 218 | sub_->subscribe(threaded_nh_, topicStr, queue_size_); 219 | sub_->registerCallback(boost::bind(&OccupancyGridDisplay::incomingMessageCallback, this, boost::placeholders::_1)); 220 | 221 | } 222 | } 223 | catch (ros::Exception& e) 224 | { 225 | setStatus(StatusProperty::Error, "Topic", (std::string("Error subscribing: ") + e.what()).c_str()); 226 | } 227 | 228 | } 229 | 230 | void OccupancyGridDisplay::unsubscribe() 231 | { 232 | clear(); 233 | 234 | try 235 | { 236 | // reset filters 237 | sub_.reset(); 238 | } 239 | catch (ros::Exception& e) 240 | { 241 | setStatus(StatusProperty::Error, "Topic", (std::string("Error unsubscribing: ") + e.what()).c_str()); 242 | } 243 | 244 | } 245 | 246 | // method taken from octomap_server package 247 | void OccupancyGridDisplay::setColor(double z_pos, double min_z, double max_z, double color_factor, 248 | rviz::PointCloud::Point& point) 249 | { 250 | int i; 251 | double m, n, f; 252 | 253 | double s = 1.0; 254 | double v = 1.0; 255 | 256 | double h = (1.0 - std::min(std::max((z_pos - min_z) / (max_z - min_z), 0.0), 1.0)) * color_factor; 257 | 258 | h -= floor(h); 259 | h *= 6; 260 | i = floor(h); 261 | f = h - i; 262 | if (!(i & 1)) 263 | f = 1 - f; // if i is even 264 | m = v * (1 - s); 265 | n = v * (1 - s * f); 266 | 267 | switch (i) 268 | { 269 | case 6: 270 | case 0: 271 | point.setColor(v, n, m); 272 | break; 273 | case 1: 274 | point.setColor(n, v, m); 275 | break; 276 | case 2: 277 | point.setColor(m, v, n); 278 | break; 279 | case 3: 280 | point.setColor(m, n, v); 281 | break; 282 | case 4: 283 | point.setColor(n, m, v); 284 | break; 285 | case 5: 286 | point.setColor(v, m, n); 287 | break; 288 | default: 289 | point.setColor(1, 0.5, 0.5); 290 | break; 291 | } 292 | } 293 | 294 | void OccupancyGridDisplay::updateTreeDepth() 295 | { 296 | updateTopic(); 297 | } 298 | 299 | void OccupancyGridDisplay::updateOctreeRenderMode() 300 | { 301 | updateTopic(); 302 | } 303 | 304 | void OccupancyGridDisplay::updateOctreeColorMode() 305 | { 306 | updateTopic(); 307 | } 308 | 309 | void OccupancyGridDisplay::updateAlpha() 310 | { 311 | updateTopic(); 312 | } 313 | 314 | void OccupancyGridDisplay::updateMaxHeight() 315 | { 316 | updateTopic(); 317 | } 318 | 319 | void OccupancyGridDisplay::updateMinHeight() 320 | { 321 | updateTopic(); 322 | } 323 | 324 | void OccupancyGridDisplay::clear() 325 | { 326 | 327 | boost::mutex::scoped_lock lock(mutex_); 328 | 329 | // reset rviz pointcloud boxes 330 | for (size_t i = 0; i < cloud_.size(); ++i) 331 | { 332 | cloud_[i]->clear(); 333 | } 334 | } 335 | 336 | void OccupancyGridDisplay::update(float wall_dt, float ros_dt) 337 | { 338 | if (new_points_received_) 339 | { 340 | boost::mutex::scoped_lock lock(mutex_); 341 | 342 | for (size_t i = 0; i < max_octree_depth_; ++i) 343 | { 344 | double size = box_size_[i]; 345 | 346 | cloud_[i]->clear(); 347 | cloud_[i]->setDimensions(size, size, size); 348 | 349 | cloud_[i]->addPoints(&new_points_[i].front(), new_points_[i].size()); 350 | new_points_[i].clear(); 351 | cloud_[i]->setAlpha(alpha_property_->getFloat()); 352 | } 353 | new_points_received_ = false; 354 | } 355 | updateFromTF(); 356 | } 357 | 358 | void OccupancyGridDisplay::reset() 359 | { 360 | clear(); 361 | messages_received_ = 0; 362 | setStatus(StatusProperty::Ok, "Messages", QString("0 binary octomap messages received")); 363 | } 364 | 365 | void OccupancyGridDisplay::updateTopic() 366 | { 367 | unsubscribe(); 368 | reset(); 369 | subscribe(); 370 | context_->queueRender(); 371 | } 372 | 373 | template 374 | bool TemplatedOccupancyGridDisplay::checkType(std::string type_id) 375 | { 376 | //General case: Need to be specialized for every used case 377 | setStatus(StatusProperty::Warn, "Messages", QString("Cannot verify octomap type")); 378 | return true; //Try deserialization, might crash though 379 | } 380 | 381 | template <> 382 | bool TemplatedOccupancyGridDisplay::checkType(std::string type_id) 383 | { 384 | if(type_id == "OcTreeStamped") return true; 385 | else return false; 386 | } 387 | template <> 388 | bool TemplatedOccupancyGridDisplay::checkType(std::string type_id) 389 | { 390 | if(type_id == "OcTree") return true; 391 | else return false; 392 | } 393 | 394 | template <> 395 | bool TemplatedOccupancyGridDisplay::checkType(std::string type_id) 396 | { 397 | if(type_id == "ColorOcTree") return true; 398 | else return false; 399 | } 400 | 401 | template 402 | void TemplatedOccupancyGridDisplay::setVoxelColor(PointCloud::Point& newPoint, 403 | typename OcTreeType::NodeType& node, 404 | double minZ, double maxZ) 405 | { 406 | OctreeVoxelColorMode octree_color_mode = static_cast(octree_coloring_property_->getOptionInt()); 407 | float cell_probability; 408 | switch (octree_color_mode) 409 | { 410 | case OCTOMAP_CELL_COLOR: 411 | setStatus(StatusProperty::Error, "Messages", QString("Cannot extract color")); 412 | //Intentional fall-through for else-case 413 | case OCTOMAP_Z_AXIS_COLOR: 414 | setColor(newPoint.position.z, minZ, maxZ, color_factor_, newPoint); 415 | break; 416 | case OCTOMAP_PROBABLILTY_COLOR: 417 | cell_probability = node.getOccupancy(); 418 | newPoint.setColor((1.0f-cell_probability), cell_probability, 0.0); 419 | break; 420 | default: 421 | break; 422 | } 423 | } 424 | 425 | //Specialization for ColorOcTreeNode, which can set the voxel color from the node itself 426 | template <> 427 | void TemplatedOccupancyGridDisplay::setVoxelColor(PointCloud::Point& newPoint, 428 | octomap::ColorOcTree::NodeType& node, 429 | double minZ, double maxZ) 430 | { 431 | float cell_probability; 432 | OctreeVoxelColorMode octree_color_mode = static_cast(octree_coloring_property_->getOptionInt()); 433 | switch (octree_color_mode) 434 | { 435 | case OCTOMAP_CELL_COLOR: 436 | { 437 | const float b2f = 1./256.; 438 | octomap::ColorOcTreeNode::Color& color = node.getColor(); 439 | newPoint.setColor(b2f*color.r, b2f*color.g, b2f*color.b, node.getOccupancy()); 440 | break; 441 | } 442 | case OCTOMAP_Z_AXIS_COLOR: 443 | setColor(newPoint.position.z, minZ, maxZ, color_factor_, newPoint); 444 | break; 445 | case OCTOMAP_PROBABLILTY_COLOR: 446 | cell_probability = node.getOccupancy(); 447 | newPoint.setColor((1.0f-cell_probability), cell_probability, 0.0); 448 | break; 449 | default: 450 | break; 451 | } 452 | } 453 | 454 | 455 | bool OccupancyGridDisplay::updateFromTF() 456 | { 457 | // get tf transform 458 | Ogre::Vector3 pos; 459 | Ogre::Quaternion orient; 460 | if (!context_->getFrameManager()->getTransform(header_, pos, orient)) { 461 | return false; 462 | } 463 | 464 | scene_node_->setOrientation(orient); 465 | scene_node_->setPosition(pos); 466 | return true; 467 | } 468 | 469 | 470 | template 471 | void TemplatedOccupancyGridDisplay::incomingMessageCallback(const octomap_msgs::OctomapConstPtr& msg) 472 | { 473 | ++messages_received_; 474 | setStatus(StatusProperty::Ok, "Messages", QString::number(messages_received_) + " octomap messages received"); 475 | setStatusStd(StatusProperty::Ok, "Type", msg->id.c_str()); 476 | if(!checkType(msg->id)){ 477 | setStatusStd(StatusProperty::Error, "Message", "Wrong octomap type. Use a different display type."); 478 | return; 479 | } 480 | 481 | ROS_DEBUG("Received OctomapBinary message (size: %d bytes)", (int)msg->data.size()); 482 | 483 | header_ = msg->header; 484 | if (!updateFromTF()) { 485 | std::stringstream ss; 486 | ss << "Failed to transform from frame [" << header_.frame_id << "] to frame [" 487 | << context_->getFrameManager()->getFixedFrame() << "]"; 488 | setStatusStd(StatusProperty::Error, "Message", ss.str()); 489 | return; 490 | } 491 | 492 | // creating octree 493 | OcTreeType* octomap = NULL; 494 | octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(*msg); 495 | if (tree){ 496 | octomap = dynamic_cast(tree); 497 | if(!octomap){ 498 | setStatusStd(StatusProperty::Error, "Message", "Wrong octomap type. Use a different display type."); 499 | } 500 | } 501 | else 502 | { 503 | setStatusStd(StatusProperty::Error, "Message", "Failed to deserialize octree message."); 504 | return; 505 | } 506 | 507 | 508 | tree_depth_property_->setMax(octomap->getTreeDepth()); 509 | 510 | // get dimensions of octree 511 | double minX, minY, minZ, maxX, maxY, maxZ; 512 | octomap->getMetricMin(minX, minY, minZ); 513 | octomap->getMetricMax(maxX, maxY, maxZ); 514 | 515 | // reset rviz pointcloud classes 516 | for (std::size_t i = 0; i < max_octree_depth_; ++i) 517 | { 518 | point_buf_[i].clear(); 519 | box_size_[i] = octomap->getNodeSize(i + 1); 520 | } 521 | 522 | size_t pointCount = 0; 523 | { 524 | // traverse all leafs in the tree: 525 | unsigned int treeDepth = std::min(tree_depth_property_->getInt(), octomap->getTreeDepth()); 526 | double maxHeight = std::min(max_height_property_->getFloat(), maxZ); 527 | double minHeight = std::max(min_height_property_->getFloat(), minZ); 528 | int stepSize = 1 << (octomap->getTreeDepth() - treeDepth); // for pruning of occluded voxels 529 | for (typename OcTreeType::iterator it = octomap->begin(treeDepth), end = octomap->end(); it != end; ++it) 530 | { 531 | if(it.getZ() <= maxHeight && it.getZ() >= minHeight) 532 | { 533 | int render_mode_mask = octree_render_property_->getOptionInt(); 534 | 535 | bool display_voxel = false; 536 | 537 | // the left part evaluates to 1 for free voxels and 2 for occupied voxels 538 | if (((int)octomap->isNodeOccupied(*it) + 1) & render_mode_mask) 539 | { 540 | // check if current voxel has neighbors on all sides -> no need to be displayed 541 | bool allNeighborsFound = true; 542 | 543 | octomap::OcTreeKey key; 544 | octomap::OcTreeKey nKey = it.getKey(); 545 | 546 | // determine indices of potentially neighboring voxels for depths < maximum tree depth 547 | // +/-1 at maximum depth, +2^(depth_difference-1) and -2^(depth_difference-1)-1 on other depths 548 | int diffBase = (it.getDepth() < octomap->getTreeDepth()) ? 1 << (octomap->getTreeDepth() - it.getDepth() - 1) : 1; 549 | int diff[2] = {-((it.getDepth() == octomap->getTreeDepth()) ? diffBase : diffBase + 1), diffBase}; 550 | 551 | // cells with adjacent faces can occlude a voxel, iterate over the cases x,y,z (idxCase) and +/- (diff) 552 | for (unsigned int idxCase = 0; idxCase < 3; ++idxCase) 553 | { 554 | int idx_0 = idxCase % 3; 555 | int idx_1 = (idxCase + 1) % 3; 556 | int idx_2 = (idxCase + 2) % 3; 557 | 558 | for (int i = 0; allNeighborsFound && i < 2; ++i) 559 | { 560 | key[idx_0] = nKey[idx_0] + diff[i]; 561 | // if rendering is restricted to treeDepth < maximum tree depth inner nodes with distance stepSize can already occlude a voxel 562 | for (key[idx_1] = nKey[idx_1] + diff[0] + 1; allNeighborsFound && key[idx_1] < nKey[idx_1] + diff[1]; key[idx_1] += stepSize) 563 | { 564 | for (key[idx_2] = nKey[idx_2] + diff[0] + 1; allNeighborsFound && key[idx_2] < nKey[idx_2] + diff[1]; key[idx_2] += stepSize) 565 | { 566 | typename OcTreeType::NodeType* node = octomap->search(key, treeDepth); 567 | 568 | // the left part evaluates to 1 for free voxels and 2 for occupied voxels 569 | if (!(node && ((((int)octomap->isNodeOccupied(node)) + 1) & render_mode_mask))) 570 | { 571 | // we do not have a neighbor => break! 572 | allNeighborsFound = false; 573 | } 574 | } 575 | } 576 | } 577 | } 578 | 579 | display_voxel |= !allNeighborsFound; 580 | } 581 | 582 | 583 | if (display_voxel) 584 | { 585 | PointCloud::Point newPoint; 586 | 587 | newPoint.position.x = it.getX(); 588 | newPoint.position.y = it.getY(); 589 | newPoint.position.z = it.getZ(); 590 | 591 | 592 | 593 | setVoxelColor(newPoint, *it, minZ, maxZ); 594 | // push to point vectors 595 | unsigned int depth = it.getDepth(); 596 | point_buf_[depth - 1].push_back(newPoint); 597 | 598 | ++pointCount; 599 | } 600 | } 601 | } 602 | } 603 | 604 | if (pointCount) 605 | { 606 | boost::mutex::scoped_lock lock(mutex_); 607 | 608 | new_points_received_ = true; 609 | 610 | for (size_t i = 0; i < max_octree_depth_; ++i) 611 | new_points_[i].swap(point_buf_[i]); 612 | 613 | } 614 | delete octomap; 615 | } 616 | 617 | } // namespace octomap_rviz_plugin 618 | 619 | #include 620 | 621 | typedef octomap_rviz_plugin::TemplatedOccupancyGridDisplay OcTreeGridDisplay; 622 | typedef octomap_rviz_plugin::TemplatedOccupancyGridDisplay ColorOcTreeGridDisplay; 623 | typedef octomap_rviz_plugin::TemplatedOccupancyGridDisplay OcTreeStampedGridDisplay; 624 | 625 | PLUGINLIB_EXPORT_CLASS( OcTreeGridDisplay, rviz::Display) 626 | PLUGINLIB_EXPORT_CLASS( ColorOcTreeGridDisplay, rviz::Display) 627 | PLUGINLIB_EXPORT_CLASS( OcTreeStampedGridDisplay, rviz::Display) 628 | 629 | -------------------------------------------------------------------------------- /src/occupancy_map_display.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2013, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Author: Julius Kammerl (jkammerl@willowgarage.com) 30 | * 31 | */ 32 | 33 | 34 | #include "octomap_rviz_plugins/occupancy_map_display.h" 35 | 36 | #include "rviz/visualization_manager.h" 37 | #include "rviz/properties/int_property.h" 38 | #include "rviz/properties/ros_topic_property.h" 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | using namespace rviz; 45 | 46 | namespace octomap_rviz_plugin 47 | { 48 | 49 | static const std::size_t max_octree_depth_ = sizeof(unsigned short) * 8; 50 | 51 | OccupancyMapDisplay::OccupancyMapDisplay() 52 | : rviz::MapDisplay() 53 | , octree_depth_ (max_octree_depth_) 54 | { 55 | 56 | topic_property_->setName("Octomap Binary Topic"); 57 | topic_property_->setMessageType(QString::fromStdString(ros::message_traits::datatype())); 58 | topic_property_->setDescription("octomap_msgs::OctomapBinary topic to subscribe to."); 59 | 60 | tree_depth_property_ = new IntProperty("Max. Octree Depth", 61 | octree_depth_, 62 | "Defines the maximum tree depth", 63 | this, 64 | SLOT (updateTreeDepth() )); 65 | } 66 | 67 | OccupancyMapDisplay::~OccupancyMapDisplay() 68 | { 69 | unsubscribe(); 70 | } 71 | 72 | void OccupancyMapDisplay::onInitialize() 73 | { 74 | rviz::MapDisplay::onInitialize(); 75 | } 76 | 77 | void OccupancyMapDisplay::updateTreeDepth() 78 | { 79 | octree_depth_ = tree_depth_property_->getInt(); 80 | } 81 | 82 | void OccupancyMapDisplay::updateTopic() 83 | { 84 | unsubscribe(); 85 | reset(); 86 | subscribe(); 87 | context_->queueRender(); 88 | } 89 | 90 | void OccupancyMapDisplay::subscribe() 91 | { 92 | if (!isEnabled()) 93 | { 94 | return; 95 | } 96 | 97 | try 98 | { 99 | unsubscribe(); 100 | 101 | const std::string& topicStr = topic_property_->getStdString(); 102 | 103 | if (!topicStr.empty()) 104 | { 105 | 106 | sub_.reset(new message_filters::Subscriber()); 107 | 108 | sub_->subscribe(threaded_nh_, topicStr, 5); 109 | sub_->registerCallback(boost::bind(&OccupancyMapDisplay::handleOctomapBinaryMessage, this, boost::placeholders::_1)); 110 | 111 | } 112 | } 113 | catch (ros::Exception& e) 114 | { 115 | setStatus(StatusProperty::Error, "Topic", (std::string("Error subscribing: ") + e.what()).c_str()); 116 | } 117 | } 118 | 119 | void OccupancyMapDisplay::unsubscribe() 120 | { 121 | clear(); 122 | 123 | try 124 | { 125 | // reset filters 126 | sub_.reset(); 127 | } 128 | catch (ros::Exception& e) 129 | { 130 | setStatus(StatusProperty::Error, "Topic", (std::string("Error unsubscribing: ") + e.what()).c_str()); 131 | } 132 | } 133 | 134 | 135 | template 136 | void TemplatedOccupancyMapDisplay::handleOctomapBinaryMessage(const octomap_msgs::OctomapConstPtr& msg) 137 | { 138 | 139 | ROS_DEBUG("Received OctomapBinary message (size: %d bytes)", (int)msg->data.size()); 140 | 141 | // creating octree 142 | OcTreeType* octomap = NULL; 143 | octomap::AbstractOcTree* tree = octomap_msgs::msgToMap(*msg); 144 | if (tree){ 145 | octomap = dynamic_cast(tree); 146 | } 147 | 148 | if (!octomap) 149 | { 150 | this->setStatusStd(StatusProperty::Error, "Message", "Failed to create octree structure"); 151 | return; 152 | } 153 | 154 | // get dimensions of octree 155 | double minX, minY, minZ, maxX, maxY, maxZ; 156 | octomap->getMetricMin(minX, minY, minZ); 157 | octomap->getMetricMax(maxX, maxY, maxZ); 158 | octomap::point3d minPt = octomap::point3d(minX, minY, minZ); 159 | 160 | unsigned int tree_depth = octomap->getTreeDepth(); 161 | 162 | octomap::OcTreeKey paddedMinKey = octomap->coordToKey(minPt); 163 | 164 | nav_msgs::OccupancyGrid::Ptr occupancy_map (new nav_msgs::OccupancyGrid()); 165 | 166 | unsigned int width, height; 167 | double res; 168 | 169 | unsigned int ds_shift = tree_depth-octree_depth_; 170 | 171 | occupancy_map->header = msg->header; 172 | occupancy_map->info.resolution = res = octomap->getNodeSize(octree_depth_); 173 | occupancy_map->info.width = width = (maxX-minX) / res + 1; 174 | occupancy_map->info.height = height = (maxY-minY) / res + 1; 175 | occupancy_map->info.origin.position.x = minX - (res / (float)(1<info.origin.position.y = minY - (res / (float)(1<data.clear(); 179 | occupancy_map->data.resize(width*height, -1); 180 | 181 | // traverse all leafs in the tree: 182 | unsigned int treeDepth = std::min(octree_depth_, octomap->getTreeDepth()); 183 | for (typename OcTreeType::iterator it = octomap->begin(treeDepth), end = octomap->end(); it != end; ++it) 184 | { 185 | bool occupied = octomap->isNodeOccupied(*it); 186 | int intSize = 1 << (octree_depth_ - it.getDepth()); 187 | 188 | octomap::OcTreeKey minKey=it.getIndexKey(); 189 | 190 | for (int dx = 0; dx < intSize; dx++) 191 | { 192 | for (int dy = 0; dy < intSize; dy++) 193 | { 194 | int posX = std::max(0, minKey[0] + dx - paddedMinKey[0]); 195 | posX>>=ds_shift; 196 | 197 | int posY = std::max(0, minKey[1] + dy - paddedMinKey[1]); 198 | posY>>=ds_shift; 199 | 200 | int idx = width * posY + posX; 201 | 202 | if (occupied) 203 | occupancy_map->data[idx] = 100; 204 | else if (occupancy_map->data[idx] == -1) 205 | { 206 | occupancy_map->data[idx] = 0; 207 | } 208 | 209 | } 210 | } 211 | 212 | } 213 | 214 | delete octomap; 215 | 216 | this->incomingMap(occupancy_map); 217 | } 218 | 219 | } // namespace rviz 220 | 221 | #include 222 | typedef octomap_rviz_plugin::TemplatedOccupancyMapDisplay OcTreeMapDisplay; 223 | typedef octomap_rviz_plugin::TemplatedOccupancyMapDisplay OcTreeStampedMapDisplay; 224 | 225 | PLUGINLIB_EXPORT_CLASS( OcTreeMapDisplay, rviz::Display) 226 | PLUGINLIB_EXPORT_CLASS( OcTreeStampedMapDisplay, rviz::Display) 227 | --------------------------------------------------------------------------------