miniatureoccupancygrid.hh

Go to the documentation of this file.

/*
 * minioccupancygrid.hh
 * Mac Mason <mmason@willowgarage.com>
 *
 * A (miniature) occupancy grid implementation. Vitally, assumes that all
 * coordinates are positive. Watch out for that.
 *
 * get and set work in meters.
 */

#pragma once
#include <boost/noncopyable.hpp>
#include <boost/thread.hpp>
#include <utility>
#include <vector>
#include "nav_msgs/OccupancyGrid.h"

namespace semanticmodel
{
  // In general, this shouldn't be noncopyable. Tough.
  class MiniatureOccupancyGrid : public boost::noncopyable
  {
    public:
      // Your choices. Like I said, miniature. Everything defaults to "empty".
      enum Occupancy { EMPTY, OCCUPIED };

      // Starts out empty. Gets bigger automatically. scale is expressed in
      // meters-per-cell. You probably want 0.01, or 0.05, or something like
      // that. 
      MiniatureOccupancyGrid(double scale = 0.05);

      // Locking for get and set; this returns a pointer which you _must
      // free_; I recommend usage like this:
      //
      // boost::scoped_ptr<boost::mutex::scoped_lock> lock(OG.get_lock());
      boost::mutex::scoped_lock* get_lock();

      // Get the value at (x, y), measured in meters. Defaults to "empty",
      // should that not exist.
      Occupancy get(double x, double y) const;

      // Set (x, y) (again in meters), to the given value. The underlying grid
      // will be expanded to make room for this.
      void set(double x, double y, Occupancy val);

      // Get the sizes (it's ragged, so it's the largest size).
      std::pair<size_t, size_t> sizes() const;

      // 2D ragged array. indexed as [x][y], after coordinate discretization.
      // Making these public makes me cry, but I'm in a hurry.
      std::vector < std::vector<Occupancy> > grid;
      double _scale;

      // Convert from world coordinates ((x, y), in meters) to grid indices
      // ((a, b), in indices). a and b are output arguments.
      void round_coordinates(double x, double y, int& a, int& b) const;

      // For publishing.
      void to_msg(nav_msgs::OccupancyGrid& og) const;

    private:
      mutable boost::mutex data_mutex;
  };
}