grid_map_2d.cpp
Go to the documentation of this file.
00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/vigir_gridmap_2d/src/GridMap2D.cpp $
00002 // SVN $Id: GridMap2D.cpp 3276 2012-09-27 12:39:16Z hornunga@informatik.uni-freiburg.de $
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 <ros/console.h>
00035 #include <vigir_footstep_planning_default_plugins/world_model/grid_map_2d.h>
00036 
00037 
00038 
00039 namespace vigir_gridmap_2d {
00040 
00041 GridMap2D::GridMap2D()
00042 : m_frameId("/map")
00043 {
00044 
00045 }
00046 
00047 GridMap2D::GridMap2D(const nav_msgs::OccupancyGridConstPtr& gridMap)
00048 {
00049   setMap(gridMap);
00050 }
00051 
00052 GridMap2D::~GridMap2D()
00053 {
00054 }
00055 
00056 void GridMap2D::setMap(const nav_msgs::OccupancyGridConstPtr& gridMap)
00057 {
00058   m_mapInfo = gridMap->info;
00059   m_frameId = gridMap->header.frame_id;
00060   // allocate map structs so that x/y in the world correspond to x/y in the image
00061   // (=> cv::Mat is rotated by 90 deg, because it's row-major!)
00062   m_binaryMap = cv::Mat(m_mapInfo.width, m_mapInfo.height, CV_8UC1);
00063   m_distMap = cv::Mat(m_binaryMap.size(), CV_32FC1);
00064 
00065   std::vector<signed char>::const_iterator mapDataIter = gridMap->data.begin();
00066 
00067   //TODO check / param
00068   unsigned char map_occ_thres = 70;
00069 
00070   // iterate over map, store in image
00071   // (0,0) is lower left corner of OccupancyGrid
00072   for(unsigned int j = 0; j < m_mapInfo.height; ++j){
00073     for(unsigned int i = 0; i < m_mapInfo.width; ++i){
00074       if (*mapDataIter > map_occ_thres){
00075         // m_mapInfo.height-1-i
00076         m_binaryMap.at<uchar>(i,j) = 0;
00077       } else{
00078         m_binaryMap.at<uchar>(i,j) = 255;
00079       }
00080       mapDataIter++;
00081     }
00082   }
00083   cv::distanceTransform(m_binaryMap, m_distMap, CV_DIST_L2, CV_DIST_MASK_PRECISE);
00084   // distance map now contains distance in meters:
00085   m_distMap = m_distMap * m_mapInfo.resolution;
00086 
00087   //ROS_INFO("GridMap2D created with %d x %d cells at %f resolution.", m_mapInfo.width, m_mapInfo.height, m_mapInfo.resolution);
00088 }
00089 
00090 void GridMap2D::setMap(const cv::Mat& binaryMap)
00091 {
00092   m_binaryMap = binaryMap.clone();
00093   m_distMap = cv::Mat(m_binaryMap.size(), CV_32FC1);
00094 
00095   cv::distanceTransform(m_binaryMap, m_distMap, CV_DIST_L2, CV_DIST_MASK_PRECISE);
00096   // distance map now contains distance in meters:
00097   m_distMap = m_distMap * m_mapInfo.resolution;
00098 
00099   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);
00100 
00101 }
00102 
00103 void GridMap2D::inflateMap(double inflationRadius)
00104 {
00105   m_binaryMap = (m_distMap <= inflationRadius );
00106   // recompute distance map with new binary map:
00107   cv::distanceTransform(m_binaryMap, m_distMap, CV_DIST_L2, CV_DIST_MASK_PRECISE);
00108   m_distMap = m_distMap * m_mapInfo.resolution;
00109 }
00110 
00111 // See costmap2D for mapToWorld / worldToMap implementations:
00112 
00113 void GridMap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
00114 {
00115   wx = m_mapInfo.origin.position.x + (mx+0.5) * m_mapInfo.resolution;
00116   wy = m_mapInfo.origin.position.y + (my+0.5) * m_mapInfo.resolution;
00117 }
00118 
00119 void GridMap2D::worldToMapNoBounds(double wx, double wy, unsigned int& mx, unsigned int& my) const
00120 {
00121   mx = (int) ((wx - m_mapInfo.origin.position.x) / m_mapInfo.resolution);
00122   my = (int) ((wy - m_mapInfo.origin.position.y) / m_mapInfo.resolution);
00123 }
00124 
00125 bool GridMap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
00126 {
00127   if(wx < m_mapInfo.origin.position.x || wy < m_mapInfo.origin.position.y)
00128     return false;
00129 
00130   mx = (int) ((wx - m_mapInfo.origin.position.x) / m_mapInfo.resolution);
00131   my = (int) ((wy - m_mapInfo.origin.position.y) / m_mapInfo.resolution);
00132 
00133   if(mx < m_mapInfo.width && my < m_mapInfo.height)
00134     return true;
00135 
00136   return false;
00137 }
00138 
00139 bool GridMap2D::inMapBounds(double wx, double wy) const
00140 {
00141   unsigned mx, my;
00142   return worldToMap(wx,wy,mx,my);
00143 }
00144 
00145 float GridMap2D::distanceMapAt(double wx, double wy) const
00146 {
00147   unsigned mx, my;
00148 
00149   if (worldToMap(wx, wy, mx, my))
00150     return m_distMap.at<float>(mx, my);
00151   else
00152     return -1.0f;
00153 }
00154 
00155 uchar GridMap2D::binaryMapAt(double wx, double wy) const
00156 {
00157   unsigned mx, my;
00158 
00159   if (worldToMap(wx, wy, mx, my))
00160     return m_binaryMap.at<uchar>(mx, my);
00161   else
00162     return 0;
00163 }
00164 
00165 float GridMap2D::distanceMapAtCell(unsigned int mx, unsigned int my) const
00166 {
00167   return m_distMap.at<float>(mx, my);
00168 }
00169 
00170 uchar GridMap2D::binaryMapAtCell(unsigned int mx, unsigned int my) const
00171 {
00172   return m_binaryMap.at<uchar>(mx, my);
00173 }
00174 
00175 bool GridMap2D::isOccupiedAtCell(unsigned int mx, unsigned int my) const
00176 {
00177   return (m_binaryMap.at<uchar>(mx, my) < 255);
00178 }
00179 
00180 bool GridMap2D::isOccupiedAt(double wx, double wy) const
00181 {
00182   unsigned mx, my;
00183   if (worldToMap(wx, wy, mx, my))
00184     return isOccupiedAtCell(mx, my);
00185   else
00186     return true;
00187 }
00188 }
00189 
00190 


vigir_footstep_planning_default_plugins
Author(s): Alexander Stumpf
autogenerated on Fri Apr 7 2017 02:59:40