miniatureoccupancygrid.cc

Go to the documentation of this file.

/*
 * miniatureoccupancygrid.cc
 * Mac Mason <mmason@willowgarage.com>
 *
 * Implementation. See miniatureoccupancygrid.hh.
 */

#include <cassert>
#include "ros/ros.h"
#include "semanticmodel/miniatureoccupancygrid.hh"

namespace semanticmodel
{
  MiniatureOccupancyGrid::MiniatureOccupancyGrid(double scale)
    : grid(), _scale(scale), data_mutex()
  {
  }

  boost::mutex::scoped_lock* MiniatureOccupancyGrid::get_lock() 
  {
    return new boost::mutex::scoped_lock(data_mutex);
  }

  // No destructor needed.
  
  MiniatureOccupancyGrid::Occupancy MiniatureOccupancyGrid::get(double x, 
                                                                double y) const
  {
    int a, b;
    round_coordinates(x, y, a, b);

    assert (x >= 0);
    assert (y >= 0);

    if ((size_t) a >= grid.size())
      return MiniatureOccupancyGrid::EMPTY;  // Default.
    else if ((size_t) b >= grid.at(a).size())
      return MiniatureOccupancyGrid::EMPTY;  // Ditto.
    else
      return grid.at(a).at(b);
  }

  void MiniatureOccupancyGrid::set(double x, double y, 
                                   MiniatureOccupancyGrid::Occupancy val) 
  {
    if ((x<=0) || (y<=0))
    {
      ROS_WARN ("Received invalid coordinates to MiniatureOccupancyGrid: %f, %f",
                x, y);
      return;
    }

    int a, b;
    round_coordinates(x, y, a, b);

    while (grid.size() <= (size_t)a)
      grid.push_back(std::vector<MiniatureOccupancyGrid::Occupancy>());

    while (grid.at(a).size() <= (size_t)b)
      grid.at(a).push_back(MiniatureOccupancyGrid::EMPTY);

    grid.at(a).at(b) = val;
  }

  std::pair<size_t, size_t> MiniatureOccupancyGrid::sizes() const
  {
    size_t ysize = 0;
    for (size_t i = 0 ; i < grid.size() ; ++i)
      ysize = std::max(ysize, grid.at(i).size());

    return std::make_pair(grid.size(), ysize);
  }

  void MiniatureOccupancyGrid::round_coordinates(double x, double y, 
                                                 int& a, int& b) const
  {
    // First, we need to convert x and y into grid units.
    double nx = x / _scale;
    double ny = y / _scale;

    a = (int)nx;
    b = (int)ny;
  }

  void MiniatureOccupancyGrid::to_msg(nav_msgs::OccupancyGrid& og) const
  {
    og.header.frame_id = "/map";
    og.header.stamp = ros::Time::now();

    og.info.map_load_time = ros::Time::now();
    og.info.resolution = _scale;
    std::pair<size_t, size_t> S = sizes();
    og.info.height = S.second;
    og.info.width = S.first;
    og.info.origin.position.x = 
    og.info.origin.position.y = 
    og.info.origin.position.z = 0;
    og.info.origin.orientation.x = 
    og.info.origin.orientation.y = 
    og.info.origin.orientation.z = 0;
    og.info.origin.orientation.w = 1.0;

    for (size_t j = 0 ; j < S.second ; ++j)
    {
      for (size_t i = 0 ; i < S.first ; ++i)
      {
        if (j < grid.at(i).size())
        {
          if (grid.at(i).at(j) == OCCUPIED)
            og.data.push_back(100);
          else
            og.data.push_back(0);
        }
        else
        {
          og.data.push_back(0);
        }
      }
    }
  }
}