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

#include <vector>

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

Step by step what we did here:

1. Created a vector of X publishers, publisher d publishes the map at depth d.
2. Check if any node is subscribing to the topic. If there is no node subscribing to the topic we do not publish on that topic.
3. If there is a node subscribing to the topic we publish on that topic at the corresponding depth.

Support Vanilla and Colored UFOMap in the Same Node

In this tutorial, we will show how you can write your node such that it supports both vanilla and colored UFOMaps. We will utilize std::variant for this.

#include <ufo/map/occupancy_map.h>
#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>

#include <variant>

// Create a UFOMap variant
// The first is std::monostate so we do not have to initialize it.
std::variant<std::monostate, ufo::map::OccupancyMap, ufo::map::OccupancyMapColor> map;

// This function checks if the current map is of correct type and has correct resolution and
// number of depth levels.
bool checkMap(std::string const &type, double resolution, ufo::map::DepthType depth_levels)
{
  return std::visit([&type, resolution, depth_levels](auto &map_l) -> bool {
    if constexpr (!std::is_same_v<std::decay_t<decltype(map_l)>, std::monostate>) {
      return map_l.getTreeType() == type && 
             map_l.getResolution() == resolution &&
	      map_l.getTreeDepthLevels() == depth_levels;
    }
    return false;
  }, map);
}

// This function creates the correct type of UFOMap based on the message metadata.
bool createMap(ufomap_msgs::UFOMapMetaData const &info)
{
  double occupied_thres = 0.5;
  double free_thres = 0.5;

  // FIXME: Remove hardcoded
  if ("occupancy_map" == info.id) {
    map.emplace<ufo::map::OccupancyMap>(info.resolution, info.depth_levels, true,
		                           occupied_thres, free_thres);
    return true;
  } else if ("occupancy_map_color" == info.id) {
    map.emplace<ufo::map::OccupancyMapColor>(info.resolution, info.depth_levels, true,
		                                occupied_thres, free_thres);
    return true;
  }
  return false;
}

void mapCallback(ufomap_msgs::UFOMapStamped::ConstPtr const& msg)
{
  // Check if we have correct map already
  if (!checkMap(msg->map.info.id, msg->map.info.resolution, msg->map.info.depth_levels)) {
    // Our map was not the same as in msg, create a new
    if (!createMap(msg->map.info)) {
      // Could not create the type of map specified in msg
      ROS_WARN("Unknown UFOMap type '%s'", msg->map.info.id.c_str());
      return;
    }
  }

  // We can access the map with std::visit.
  // Here we simply convert the message into our map.
  std::visit([&msg](auto &map_l) {
    if constexpr (!std::is_same_v<std::decay_t<decltype(map_l)>, std::monostate>) {
      // Convert ROS message to UFOMap
      if (ufomap_msgs::msgToUfo(msg->map, map_l)) {
        // Conversion was successful
        ROS_INFO("UFOMap conversion successful");
      } else {
        ROS_WARN("UFOMap conversion failed");
      }
    }
  }, map);
}

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

  ros::Subscriber map_sub = nh.subscribe("map", 10, mapCallback);

  ros::spin();

  return 0;
}

Here we created two helper functions to check if our current map is of the same type as the message in our callback and then another function to create a UFOMap of the correct type based on the current message in the callback.

It is recommended to read up about std::variant and std::visit if this code is confusing.