ROS Tutorials

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

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