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.

Again, these are not complete examples. In the examples variables will be accessed in ways it is not possible. This is to reduce the amount of code needed for the examples. When you implemented these things you will probably have classes, so the map will be a class member that you can access from member functions.

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.

Nothing on the subscribe side has to be changed for this to work, since all information is included in the message.

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.

// 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>

// Function that publishes the whole map to new nodes that subscribe to the map
void mapConnectCallback(ros::SingleSubscriberPublisher const &pub) 
{
  ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped);
  // Convert UFOMap to ROS, with compression and at depth 0. You can of course change these.
  if (ufomap_msgs::ufoToMsg(map, msg->map, true, 0)) {
    // Conversion was successful
    msg->header.stamp = ros::Time::now();
    msg->header.frame_id = "map";
    // Publish the whole map to the specific node that just subscribed
    pub.publish(msg);
  }
}

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

  // Advance publisher. 
  ros::Publisher map_pub = nh.advertise<ufomap_msgs::UFOMapStamped>("map", 10, &mapConnectCallback, 
                                                                    ros::SubscriberStatusCallback(), 
                                                                    ros::VoidConstPtr());

  // 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. Used the advance ROS publisher constructor, to setup a callback for when a new nodes subscribes to the topic. The line ros::Publisher map_pub = nh.advertise<ufomap_msgs::UFOMapStamped>("map", 10, &mapConnectCallback, ros::SubscriberStatusCallback(), ros::VoidConstPtr());
2. Implemented the mapConnectCallback function that publishes the whole map to a single node that subscribes.

Again, nothing on the subscribe side has to be changed for this to work.

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.

// 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>

// Function that publishes the whole map to new nodes that subscribe to the map.
// d is the depth of the map we should publish.
void mapConnectCallback(ros::SingleSubscriberPublisher const &pub, int d) 
{
  ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped);
  // Convert UFOMap to ROS, with compression and at depth d. You can of course change these.
  if (ufomap_msgs::ufoToMsg(map, msg->map, true, d)) {
    // Conversion was successful
    msg->header.stamp = ros::Time::now();
    msg->header.frame_id = "map";
    // Publish the whole map to the specific node that just subscribed
    pub.publish(msg);
  }
}

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

  size_t max_depth_to_pub = 4;

  // Create a number of publishers.
  std::vector<ros::Publisher> map_pubs(max_depth_to_pub);

  // Construct publishers.
  for (int d = 0; d < map_pubs.size(); ++d) {
    std::string final_topic = d == 0 ? "map" : "map_depth_" + std::to_string(d);
    // Advance publisher
    map_pubs[d] = nh.advertise<ufomap_msgs::UFOMapStamped>(final_topic, 10, 
                                                           boost::bind(&mapConnectCallback, _1, d),
                                                           ros::SubscriberStatusCallback(), 
                                                           ros::VoidConstPtr());
  }

  // 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;

  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());

    for (int d = 0; d < map_pubs.size(); ++d) {
      if (!map_pubs[d] || (0 >= map_pubs[d].getNumSubscribers() && !map_pubs[d].isLatched())) {
        // This publisher is broken(?) or
        // There are no subscribers on this topic and the topic is not latched
        continue;
      }

      // 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, d)) {
        // Conversion was successful
        msg->header.stamp = ros::Time::now();
        msg->header.frame_id = "map";
        map_pubs[d].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;
}