GridMap2D.cpp
Go to the documentation of this file.
00001 // SVN $HeadURL$
00002 // SVN $Id$
00003 
00004 /*
00005  * A simple 2D gridmap structure
00006  *
00007  * Copyright 2011 Armin Hornung, University of Freiburg
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00034 #include "gridmap_2d/GridMap2D.h"
00035 #include <ros/console.h>
00036 
00037 namespace gridmap_2d{
00038 
00039 GridMap2D::GridMap2D()
00040 : m_frameId("/map")
00041 {
00042 
00043 }
00044 
00045 GridMap2D::GridMap2D(const nav_msgs::OccupancyGridConstPtr& gridMap) {
00046 
00047   setMap(gridMap);
00048 
00049 }
00050 
00051 GridMap2D::~GridMap2D() {
00052 
00053 }
00054 
00055 void GridMap2D::setMap(const nav_msgs::OccupancyGridConstPtr& gridMap){
00056   m_mapInfo = gridMap->info;
00057   m_frameId = gridMap->header.frame_id;
00058   // allocate map structs so that x/y in the world correspond to x/y in the image
00059   // (=> cv::Mat is rotated by 90 deg, because it's row-major!)
00060   m_binaryMap = cv::Mat(m_mapInfo.width, m_mapInfo.height, CV_8UC1);
00061   m_distMap = cv::Mat(m_binaryMap.size(), CV_32FC1);
00062 
00063   std::vector<signed char>::const_iterator mapDataIter = gridMap->data.begin();
00064 
00065   //TODO check / param
00066   unsigned char map_occ_thres = 70;
00067 
00068   // iterate over map, store in image
00069   // (0,0) is lower left corner of OccupancyGrid
00070   for(unsigned int j = 0; j < m_mapInfo.height; ++j){
00071     for(unsigned int i = 0; i < m_mapInfo.width; ++i){
00072       if (*mapDataIter > map_occ_thres){
00073         // m_mapInfo.height-1-i
00074         m_binaryMap.at<uchar>(i,j) = 0;
00075       } else{
00076         m_binaryMap.at<uchar>(i,j) = 255;
00077       }
00078       mapDataIter++;
00079     }
00080   }
00081   cv::distanceTransform(m_binaryMap, m_distMap, CV_DIST_L2, CV_DIST_MASK_PRECISE);
00082   // distance map now contains distance in meters:
00083   m_distMap = m_distMap * m_mapInfo.resolution;
00084 
00085   ROS_INFO("GridMap2D created with %d x %d cells at %f resolution.", m_mapInfo.width, m_mapInfo.height, m_mapInfo.resolution);
00086 }
00087 
00088 void GridMap2D::setMap(const cv::Mat& binaryMap){
00089   m_binaryMap = binaryMap.clone();
00090   m_distMap = cv::Mat(m_binaryMap.size(), CV_32FC1);
00091 
00092   cv::distanceTransform(m_binaryMap, m_distMap, CV_DIST_L2, CV_DIST_MASK_PRECISE);
00093   // distance map now contains distance in meters:
00094   m_distMap = m_distMap * m_mapInfo.resolution;
00095 
00096   ROS_INFO("GridMap2D copied from existing cv::Mat with %d x %d cells at %f resolution.", m_mapInfo.width, m_mapInfo.height, m_mapInfo.resolution);
00097 
00098 }
00099 
00100 void GridMap2D::inflateMap(double inflationRadius){
00101   m_binaryMap = (m_distMap > inflationRadius );
00102   // recompute distance map with new binary map:
00103   cv::distanceTransform(m_binaryMap, m_distMap, CV_DIST_L2, CV_DIST_MASK_PRECISE);
00104   m_distMap = m_distMap * m_mapInfo.resolution;
00105 }
00106 
00107 // See costmap2D for mapToWorld / worldToMap implementations:
00108 
00109 void GridMap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const {
00110   wx = m_mapInfo.origin.position.x + (mx+0.5) * m_mapInfo.resolution;
00111   wy = m_mapInfo.origin.position.y + (my+0.5) * m_mapInfo.resolution;
00112 }
00113 
00114 
00115 
00116 void GridMap2D::worldToMapNoBounds(double wx, double wy, unsigned int& mx, unsigned int& my) const {
00117   mx = (int) ((wx - m_mapInfo.origin.position.x) / m_mapInfo.resolution);
00118   my = (int) ((wy - m_mapInfo.origin.position.y) / m_mapInfo.resolution);
00119 }
00120 
00121 bool GridMap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const {
00122   if(wx < m_mapInfo.origin.position.x || wy < m_mapInfo.origin.position.y)
00123     return false;
00124 
00125   mx = (int) ((wx - m_mapInfo.origin.position.x) / m_mapInfo.resolution);
00126   my = (int) ((wy - m_mapInfo.origin.position.y) / m_mapInfo.resolution);
00127 
00128   if(mx < m_mapInfo.width && my < m_mapInfo.height)
00129     return true;
00130 
00131   return false;
00132 }
00133 
00134 bool GridMap2D::inMapBounds(double wx, double wy) const{
00135   unsigned mx, my;
00136   return worldToMap(wx,wy,mx,my);
00137 }
00138 
00139 float GridMap2D::distanceMapAt(double wx, double wy) const{
00140   unsigned mx, my;
00141 
00142   if (worldToMap(wx, wy, mx, my))
00143     return m_distMap.at<float>(mx, my);
00144   else
00145     return -1.0f;
00146 }
00147 
00148 
00149 uchar GridMap2D::binaryMapAt(double wx, double wy) const{
00150   unsigned mx, my;
00151 
00152   if (worldToMap(wx, wy, mx, my))
00153     return m_binaryMap.at<uchar>(mx, my);
00154   else
00155     return 0;
00156 }
00157 
00158 
00159 float GridMap2D::distanceMapAtCell(unsigned int mx, unsigned int my) const{
00160   return m_distMap.at<float>(mx, my);
00161 }
00162 
00163 
00164 uchar GridMap2D::binaryMapAtCell(unsigned int mx, unsigned int my) const{
00165   return m_binaryMap.at<uchar>(mx, my);
00166 }
00167 
00168 
00169 bool GridMap2D::isOccupiedAtCell(unsigned int mx, unsigned int my) const{
00170   return (m_binaryMap.at<uchar>(mx, my) < 255);
00171 }
00172 
00173 
00174 bool GridMap2D::isOccupiedAt(double wx, double wy) const{
00175   unsigned mx, my;
00176   if (worldToMap(wx, wy, mx, my))
00177     return isOccupiedAtCell(mx, my);
00178   else
00179     return true;
00180 }
00181 
00182 }
00183 
00184 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


gridmap_2d
Author(s): Armin Hornung
autogenerated on Tue Oct 15 2013 10:06:42