Octomap Navigation? How to?

edit

Hello RoboRos,

this is actually a very interesting question.
There exists a C++ library that could help you with your task. It is called grid_map. As stated on the link provided, the grid_map is:

... a C++ library with ROS interface to manage two-dimensional grid maps with multiple data layers. It is designed for mobile robotic mapping to store data such as elevation, variance, color, friction coefficient, foothold quality, surface normal, traversability etc.

Since using grid maps would be an entirely different topic, I will try to keep it short as possible by pointing out that there is a demonstration of an octomap conversion provided here. After installing the grid_map package, just change the path in the given launch file to point to your map - it will give you a quick demonstration of just what you could do with it.

However, I would advise you to play a bit with the package since the entire concept of grip maps is very interesting and above all, very useful.

If you have any further question, feel free to ask.

EDIT: To save the image of the conversion, in grid_map_demos/src/OctomapToGridmapDemo.cpp add the following lines of code:

#include <opencv2/imgcodecs.hpp>
#include "grid_map_cv/grid_map_cv.hpp"

You need to include opencv in order to use imwrite and grid_map_cv is needed, because it offers the function for grid map to image conversion. Then, find this part (it already exists):

// Publish as grid map.
grid_map_msgs::GridMap gridMapMessage;
grid_map::GridMapRosConverter::toMessage(map_, gridMapMessage);
gridMapPublisher_.publish(gridMapMessage);

At that point, grid map is already constructed and you can save it by adding the following:

cv::Mat map_img;
grid_map::GridMapCvConverter::toImage<unsigned char,1>(map_, "elevation", CV_8UC1, map_img);
cv::imwrite("/home/<username>/octomap_occupancy.pgm", map_img);

You create a standard OpenCV Mat object, call the template function for grid map conversion with all the arguments and then just save the resulting image. toImage function declaration is:

template<typename Type_, int NChannels_>
static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer, const int encoding, cv::Mat& image)