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.

CmakeLists.txt and package.xml

To use UFOMap in your package you need to add

find_package(ufomap REQUIRED)

target_link_libraries(YOUR_NODE
  ${catkin_LIBRARIES}
  UFO::Map
)

to your CMakeLists.txt and

<build_depend>ufomap</build_depend>
<run_depend>ufomap</run_depend>

to your package.xml.

If your package also requires ufomap_msgs and/or ufomap_ros you can add them as standard catkin components

find_package(catkin REQUIRED COMPONENTS
  roscpp
  ufomap_msgs
  ufomap_ros
)

to your CMakeLists.txt and

<build_depend>ufomap_msgs</build_depend>
<build_depend>ufomap_ros</build_depend>
<run_depend>ufomap_msgs</run_depend>
<run_depend>ufomap_ros</run_depend>

to your package.xml.

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: Integrate data into map

    // 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_trans = 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_trans.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;
}

Perform Mapping

It is recommended to use the UFOMap mapping server if you want to easily perform mapping. The mapping server listens for sensor_msgs/PointCloud2 and publishes UFOMap. It also provides a number of services and is highly configurable. It is possible to configure the mapping server while it is running using dynamic reconfigure. Go to the UFOMap mapping server page to read more.

Visualize UFOMap in RViz

In the video below you can see how to use the UFOMap RViz plugin. Because of the number of voxels to display when mapping at small voxel sizes (2 cm and below), RViz gets slow. An update to the UFOMap RViz plugin, that is faster, is in development.

Convert Between UFOMap and OctoMap

TODO

General UFOMap Usage

For general UFOMap usage see Tutorials.

Advanced ROS Tutorials

Next, you can look at advanced UFOMap ROS tutorials.