Advanced ROS Tutorials

Introduction

Here are advanced examples of how UFOMap can be used with ROS. Most of the things mentioned here are already implemented in the UFOMap mapping server, so by used that you already get all of these benefits.

Publish Only Updated Part of Map

Often only a small portion of the map will be updated at a time. To increase performance and reduce the amount of data that has to be published between nodes, it is possible to define which part of the map should be published.

// We will publish colored UFOMaps in this tutorial
#include <ufo/map/occupancy_map_color.h>
// UFOMap ROS msg
#include <ufomap_msgs/UFOMapStamped.h>
// To convert between UFO and ROS
#include <ufomap_msgs/conversions.h>
#include <ufomap_ros/conversions.h>

#include <ros/ros.h>

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "ufomap_publisher");
  ros::NodeHandle nh;

  ros::Publisher map_pub = nh.advertise<ufomap_msgs::UFOMapStamped>("map", 10);

  // Create a colored UFOMap
  ufo::map::OccupancyMapColor map(0.1);

  // Enable min/max change detection. This allows us to get an 
  // axis-aligned bounding box of the updated region.
  map.enableMinMaxChangeDetection(true);

  // If the UFOMap should be compressed using LZ4.
  // Good if you are sending the UFOMap between computers.
  bool compress = false;

  // Lowest depth to publish.
  // Higher value means less data to transfer, good in
  // situation where the data rate is low.
  // Many nodes do not require detailed maps as well.
  ufo::map::DepthType pub_depth = 0;

  ros::Rate rate(10);
  while (ros::ok()) {
    // TODO: Integrate data into map

    // Get axis-aligned bounding box of update part of the map.
    ufo::geometry::AABB updated_aabb(map.minChange(), map.maxChange());

    // This is the UFOMap message object.
    ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped);
    // Convert UFOMap to ROS message. Here we add the axis-aligned bounding box,
    // which tells us what part of the UFOMap we want to publish.
    if (ufomap_msgs::ufoToMsg(map, msg->map, updated_aabb, compress, pub_depth)) {
      // Conversion was successful
      msg->header.stamp = ros::Time::now();
      msg->header.frame_id = "map";
      map_pub.publish(msg);		        
      
      // Reset min/max change detection so we get correct axis-aligned bounding box
      // next time we publish.
      map.resetMinMaxChangeDetection();
    }

    rate.sleep();
  }

  return 0;
}

Step by step what we did here:

1. Enabled min/max change detection with the line map.enableMinMaxChangeDetection(true);, this allows us to get the axis-aligned bounding box of the updated part of the map.
2. Retrieved the axis-aligned bounding box of the updated part of the map, with the line ufo::geometry::AABB updated_aabb(map.minChange(), map.maxChange());, before we convert UFOMap to ROS message.
3. Insert the axis-aligned bounding box in the UFO to ROS conversion function ufomap_msgs::ufoToMsg(map, msg->map, updated_aabb, compress, pub_depth). The msg will now contain information about this axis-aligned bounding box, inside msg->map.info.bounding_volume.aabbs, such that the nodes that subscribe to this topic will know what part of the map they should update with this message.
4. Reset the min/max change detection, with the line map.resetMinMaxChangeDetection();, such that we can use it again next time we should publish.

Publish Full Map to New Subscribers

If you followed the previous tutorial and now only publish the updated part of the map, you will notice that when nodes subscribe to the map at different times they will not receive the full map. In order to solve this, we have to detect when a new node is subscribing to our map and send the whole map to that node.

Another approach to solving this problem can be to publish the whole map every X seconds; however, when mapping at 5 mm the map will get huge, so this will eventually get slow.

Publish Multiple Depths

Mapping at a fine voxel size (2 cm or below) results in a lot of data. In a complete robotics system, different nodes require different map resolutions. It can therefore be beneficial to publish the map at different depths, and only publish at a specific depth if a node subscribes to the topic for that depth.