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


gridmap_2d
Author(s): Armin Hornung
autogenerated on Mon Oct 6 2014 00:41:23