ROS Tutorials

Introduction

Here are examples of how to use UFOMap with ROS. These are not complete examples, it is therefore not possible to run them as is. They are simply to showcase how to do certain things.

UFOMap publisher

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

  // 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: Fill in map with some data

    // This is the UFOMap message object.
    ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped);
    // Convert UFOMap to ROS message
    if (ufomap_msgs::ufoToMsg(map, msg->map, compress, pub_depth)) {
      // Conversion was successful
      msg->header.stamp = ros::Time::now();
      msg->header.frame_id = "map";
      map_pub.publish(msg);					        
    }

    rate.sleep();
  }

  return 0;
}

UFOMap subscriber

// We will subscribe to 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>

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

void mapCallback(ufomap_msgs::UFOMapStamped::ConstPtr const& msg)
{
  // Convert ROS message to UFOMap
  if (ufomap_msgs::msgToUfo(msg->map, map)) {
    // Conversion was successful
    ROS_INFO("UFOMap conversion successful");
  } else {
    ROS_WARN("UFOMap conversion failed");
  }
}

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

Integrate sensor_msgs/PointCloud2

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

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_listener.h>

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

// TF listener
tf2_ros::Buffer tf_buffer;
tf2_ros::TransformListener tf_listener(tf_buffer);

void mapCallback(sensor_msgs::PointCloud2::ConstPtr const &msg)
{
  // Get transform
  ufo::math::Pose6 transform;
  try {
    // Lookup transform
    geometry_msgs::TransformStamped tf_transform = tf_buffer_.lookupTransform("map", msg->header.frame_id, msg->header.stamp, ros::Duration(0.1));
    // Convert ROS transform to UFO transform
    transform = ufomap_ros::rosToUfo(tf_transform.transform);
  } catch (tf2::TransformException &ex) {
    ROS_WARN_THROTTLE(1, "%s", ex.what());
    return;
  }

  ufo::map::PointCloudColor cloud;
  // Convert ROS point cloud to UFO point cloud
  ufomap_ros::rosToUfo(*msg, cloud);
  // Transform point cloud to correct frame, do it in parallel (second param true)
  cloud.transform(transform, true);

  // Integrate point cloud into UFOMap, no max range (third param -1), free space at depth level 1 (fourth param 1)
  map.insertPointCloudDiscrete(transform.translation(), cloud, -1, 1);
}

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

  ros::Subscriber cloud_sub = nh.subscribe("cloud", 10, cloudCallback);

  ros::spin();

  return 0;
}

Visualize UFOMap in RViz

TODO: Upload video showing this

General UFOMap usage

For general UFOMap usage see Tutorials.