leonardlohky / volumetric_mapping

volumetric_mapping

A repository for 3D volumetric (occupancy) maps, providing a generic interface for disparity map and pointcloud insertion, and support for custom sensor error models.

Packages

volumetric_map_base - base class/package that all volumetric maps should inherit from, contains methods to handle ROS disparity maps and pointclouds.

volumetric_msgs - collection of messages for interacting with various maps.

octomap_world - an octomap-based volumetric representation, both with a library and a stand-alone ROS node.

Dependencies

OpenCV is required for this package to work, installation of this package was tested with OpenCV version 3.4.14. See instructions on how to install OpenCV on Ubuntu system

In addition to ros-indigo-desktop-full, please install:

sudo apt-get install ros-indigo-octomap-mapping

And the following packages, which can be done all at once with the script below:
minkindr
minkindr_ros
eigen_catkin
eigen_checks
glog_catkin
gflags_catkin

cd ~/catkin_ws/src/
wstool init
wstool set catkin_simple       --git https://github.com/catkin/catkin_simple.git
wstool set eigen_catkin        --git https://github.com/ethz-asl/eigen_catkin.git
wstool set eigen_checks        --git https://github.com/ethz-asl/eigen_checks.git
wstool set gflags_catkin       --git https://github.com/ethz-asl/gflags_catkin.git
wstool set glog_catkin         --git https://github.com/ethz-asl/glog_catkin.git
wstool set minkindr            --git https://github.com/ethz-asl/minkindr.git
wstool set minkindr_ros        --git https://github.com/ethz-asl/minkindr_ros.git
wstool set volumetric_mapping  --git https://github.com/ethz-asl/volumetric_mapping.git
wstool update

On Mac OS X, see the mav_tools Wiki instructions.

Installation

Assuming SegMap and its workspace is already installed, most of the required dependencies should already be in the ~/segmap_ws/src folder. So, the package can be simply git cloned into the ~/segmap_ws/src folder

cd ~/segmap_ws/src/
git clone https://github.com/leonardlohky/volumetric_mapping

After that, build the package using catkin build

cd ~/segmap_ws
catkin build volumetric_mapping

Libraries

OctomapWorld - general library for handling insertion of pointclouds, can be run outside of a ROS node, and takes parameters as a struct.

OctomapManager - inherits from OctomapWorld, essentially a ROS wrapper for it. Reads parameters in from the ROS parameter server.

Nodes

octomap_manager

Listens to disparity and pointcloud messages and adds them to an octomap.

Parameters

• tf_frame (string, default: "/world") - tf frame name to use for the world.
• resolution (double, default: 0.15) - resolution each grid cell in meters.
• Q (vector of doubles (representing 4x4 matrix, row-major)) - Q projection matrix for disparity projection, in case camera info topics are not available.
• map_publish_frequency (double, default: 0.0) - Frequency at which the Octomap is published for visualization purposes. If set to < 0.0, the Octomap is not regularly published (use service call instead).
• octomap_file (string, default: "") - Loads an octomap from this path on startup. Use load_map service below to load a map from file after startup.

For other parameters, see octomap_world.h.

Subscribed Topics

• disparity (stereo_msgs/DisparityImage) - disparity image to subscribe to.
• pointcloud (sensor_msgs/PointCloud2) - pointcloud to subscribe to.
• cam0/camera_info (sensor_msgs/CameraInfo) - left camera info.
• cam1/camera_info (sensor_msgs/CameraInfo) - right camera info.

Published Topics

• octomap_occupied (visualization_msgs/MarkerArray) - marker array showing occupied octomap cells, colored by z.
• octomap_free (visualization_msgs/MarkerArray) - marker array showing free octomap cells, colored by z.
• octomap_full (octomap_msgs/Octomap) - octomap with full probabilities.
• octomap_binary (octomap_msgs/Octomap) - octomap with binary occupancy - free or occupied, taken by max likelihood of each node.

Services

• reset_map (std_srvs/Empty) - clear the map.
• publish_all (std_srvs/Empty) - publish all the topics in the above section.
• get_map (octomap_msgs/GetOctomap) - returns binary octomap message.
• save_map (volumetric_msgs/SaveMap) - save map to the specified file_path.
• load_map (volumetric_msgs/LoadMap) - load map from the specified file_path.

Running

Run an octomap manager, and load a map from disk, then publish it in the map tf frame:

rosrun octomap_world octomap_manager _tf_frame:=map
rosservice call /octomap_manager/load_map /home/helen/data/my_awesome_octomap.bt
rosservice call /octomap_manager/publish_all