C++ GridMap类代码示例

//call this method only
void TerrainInfo::CleanUpGrids(const uint32 diff)
{
    i_timer.Update(diff);
    if( !i_timer.Passed() )
        return;

    for (int y = 0; y < MAX_NUMBER_OF_GRIDS; ++y)
    {
        for (int x = 0; x < MAX_NUMBER_OF_GRIDS; ++x)
        {
            const int16& iRef = m_GridRef[x][y];
            GridMap * pMap = m_GridMaps[x][y];

            //delete those GridMap objects which have refcount = 0
            if(pMap && iRef == 0 )
            {
                m_GridMaps[x][y] = NULL;
                //delete grid data if reference count == 0
                pMap->unloadData();
                delete pMap;

                //unload VMAPS...
                VMAP::VMapFactory::createOrGetVMapManager()->unloadMap(m_mapId, x, y);

                //unload mmap...
                MMAP::MMapFactory::createOrGetMMapManager()->unloadMap(m_mapId, x, y);
            }
        }
    }

    i_timer.Reset();
}

double MaxAreaHandler::explorationPotential(GridMap& m, QPoint position, int radius)
{
	double result = 0.0;
	
	if (m.isValidField(position))
	{
		return 0.0;
	}
	
	int xStart = position.x() - radius;
	int xEnd = position.x() + radius;
	int yStart = position.y() - radius;
	int yEnd = position.y() + radius;
	
	for ( int a = xStart; a <= xEnd; ++a) {
		for (int b = yStart; b <= yEnd; ++b) {
			if (m.isValidField(a, b)) {
				
			}
			else {
				
			}
		}
	}
	return result;
}

float MapTerrain2::GetHeight(float x, float y, float z)
{
	GridMap *g = GetGrid(x, y);
	if( g )
		return g->getHeight( x, y );
	return VMAP_VALUE_NOT_INITIALIZED;
}

void BasicVFF::calcRepulsiveForceSum(const GridMap &map)
{
    for(int i = 0; i < map.getMapSize().y; i++)
    {
        for(int j = 0; j < map.getMapSize().x; j++)
        {
			uchar occupancyData = map.getOccupancyGridMap()[i * map.getMapSize().x + j];
            if(occupancyData > 0)
            {
                Position2f ObstaclePos;
                map.getPositionFromIndex(Cell2i(i,j), ObstaclePos);
                //cout << i << " " << j << endl;
                //cout << ObstaclePos.x << " " << ObstaclePos.y << endl;
                float distancePow = getDistancePow(ObstaclePos);
                float distance = powf(distancePow, 0.5);
                force2f force = force2f((0 - ObstaclePos.x) / distance , (0 - ObstaclePos.y) / distance);
                float coeff = (float)occupancyData / distancePow;
                //cout << (float)occupancyData << " "<< distancePow << " " << coeff<< endl;

                force2f force1 = coeff * force;
                forceSum += force1;
                //cout << force.x << " " << force.y << endl;
                //cout << force1.x << " " << force1.y << endl;
            }
        }
    }

    cout << "ForceSum = " << forceSum.x << " "<< forceSum.y << endl;
}

void PotentialField::target_waypoint_callback(
    visualization_msgs::Marker::ConstPtr target_point_msgs) {
  static TargetWaypointFieldParamater param;
  double ver_x_p(param.ver_x_p);
  double ver_y_p(param.ver_y_p);
  ros::Time time = ros::Time::now();
  geometry_msgs::PointStamped in, out;
  in.header = target_point_msgs->header;
  in.point = target_point_msgs->pose.position;
  tf::TransformListener tflistener;
  try {
    ros::Time now = ros::Time(0);
    tflistener.waitForTransform("/map", "/potential_field_link", now,
                                ros::Duration(10.0));
    tflistener.transformPoint("/potential_field_link", in.header.stamp, in,
                              "/map", out);

  } catch (tf::TransformException &ex) {
    ROS_ERROR("%s", ex.what());
  }

  for (GridMapIterator it(map_); !it.isPastEnd(); ++it) {
    Position position;
    map_.getPosition(*it, position);
    map_.at("target_waypoint_field", *it) = 0.0;
    map_.at("target_waypoint_field", *it) -=
        0.5 * std::exp((-1.0 * (std::pow((position.y() - (out.point.y)), 2.0) /
                                std::pow(2.0 * ver_y_p, 2.0))) +
                       (-1.0 * (std::pow((position.x() - (out.point.x)), 2.0) /
                                std::pow(2.0 * ver_x_p, 2.0))));
  }
  map_.setTimestamp(time.toNSec());
}

FrontierList Planner::getFrontiers(GridMap* map, GridPoint start)
{
	// Initialization
	mFrontierCellCount = 0;
	mFrontierCount = 0;
	GridMap plan = GridMap(map->getWidth(), map->getHeight());
	FrontierList result;
	
	// Initialize the queue with the robot position
	Queue queue;
	queue.insert(Entry(0, start));
	plan.setData(start, VISIBLE);
	
	// Do full search with weightless Dijkstra-Algorithm
	while(!queue.empty())
	{		
		// Get the nearest cell from the queue
		Queue::iterator next = queue.begin();
		int distance = next->first;
		GridPoint point = next->second;
		queue.erase(next);
		
		// Add neighbors
		bool isFrontier = false;
		PointList neighbors = getNeighbors(point, false);
        char c = 0;
		for(PointList::const_iterator cell = neighbors.begin(); cell < neighbors.end(); cell++)
		{   
            if(map->getData(*cell,c) && c == UNKNOWN)
            {
                plan.setData(*cell, OBSTACLE);
                isFrontier = true;
                break;
            }
            if(map->getData(*cell, c) && c == VISIBLE && 
                plan.getData(*cell, c) && c == UNKNOWN)
            {
                queue.insert(Entry(distance+1, *cell));
                plan.setData(*cell, VISIBLE);
            }
		}
		
		if(isFrontier)
		{
			result.push_back(getFrontier(map, &plan, point));
		}
	}
	
	// Set result message and return the point list
	if(result.size() > 0)
	{
		mStatus = SUCCESS;
		sprintf(mStatusMessage, "Found %d frontiers with %d frontier cells.", mFrontierCount, mFrontierCellCount);
	}else
	{
		mStatus = NO_GOAL;
		sprintf(mStatusMessage, "No reachable frontiers found.");
	}
	return result;
}

Nona7::GridMap Nona7::nextPossibleWorld(Board::Grid grid){
    GridMap map;
    GridList lefts = nextPossibleWorldLeft(grid);
    for(auto e: lefts) map.push_back(std::make_pair(e, Dir::Left));
    GridList right = nextPossibleWorldLeft(Board::gridMirror(grid));
    for(auto e: right) map.push_back(std::make_pair(Board::gridMirror(e), Dir::Right));
    GridList down = nextPossibleWorldLeft(Board::gridMirror(Board::transpose(grid)));
    for(auto e: down) map.push_back(std::make_pair(Board::transpose(Board::gridMirror(e)), Dir::Down));
    GridList up = nextPossibleWorldLeft(Board::transpose(grid));
    for(auto e: up) map.push_back(std::make_pair(Board::transpose(e), Dir::Up));
    return map;
}

GridMap * TerrainInfo::LoadMapAndVMap( const uint32 x, const uint32 y )
{
    //double checked lock pattern
    if(!m_GridMaps[x][y])
    {
        LOCK_GUARD lock(m_mutex);

        if(!m_GridMaps[x][y])
        {
            GridMap * map = new GridMap();

            // map file name
            char *tmp=NULL;
            int len = sWorld.GetDataPath().length()+strlen("maps/%03u%02u%02u.map")+1;
            tmp = new char[len];
            snprintf(tmp, len, (char *)(sWorld.GetDataPath()+"maps/%03u%02u%02u.map").c_str(),m_mapId, x, y);
            sLog.outDetail("Loading map %s",tmp);

            if(!map->loadData(tmp))
            {
                sLog.outError("Error load map file: \n %s\n", tmp);
                //ASSERT(false);
            }

            delete [] tmp;
            m_GridMaps[x][y] = map;

            //load VMAPs for current map/grid...
            const MapEntry * i_mapEntry = sMapStore.LookupEntry(m_mapId);
            const char* mapName = i_mapEntry ? i_mapEntry->name[sWorld.GetDefaultDbcLocale()] : "UNNAMEDMAP\x0";

            int vmapLoadResult = VMAP::VMapFactory::createOrGetVMapManager()->loadMap((sWorld.GetDataPath()+ "vmaps").c_str(),  m_mapId, x, y);
            switch(vmapLoadResult)
            {
            case VMAP::VMAP_LOAD_RESULT_OK:
                sLog.outDetail("VMAP loaded name:%s, id:%d, x:%d, y:%d (vmap rep.: x:%d, y:%d)", mapName, m_mapId, x,y,x,y);
                break;
            case VMAP::VMAP_LOAD_RESULT_ERROR:
                sLog.outDetail("Could not load VMAP name:%s, id:%d, x:%d, y:%d (vmap rep.: x:%d, y:%d)", mapName, m_mapId, x,y,x,y);
                break;
            case VMAP::VMAP_LOAD_RESULT_IGNORED:
                DEBUG_LOG("Ignored VMAP name:%s, id:%d, x:%d, y:%d (vmap rep.: x:%d, y:%d)", mapName, m_mapId, x,y,x,y);
                break;
            }

            // load navmesh
            MMAP::MMapFactory::createOrGetMMapManager()->loadMap(m_mapId, x, y);
        }
    }

    return  m_GridMaps[x][y];
}

bool ElevationMap::publishFusedElevationMap()
{
  if (!hasFusedMapSubscribers()) return false;
  boost::recursive_mutex::scoped_lock scopedLock(fusedMapMutex_);
  GridMap fusedMapCopy = fusedMap_;
  scopedLock.unlock();
  fusedMapCopy.add("uncertainty_range", fusedMapCopy.get("upper_bound") - fusedMapCopy.get("lower_bound"));
  grid_map_msgs::GridMap message;
  GridMapRosConverter::toMessage(fusedMapCopy, message);
  elevationMapFusedPublisher_.publish(message);
  ROS_DEBUG("Elevation map (fused) has been published.");
  return true;
}

void SignedDistanceField::calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer,
                                                       const double heightClearance)
{
  data_.clear();
  resolution_ = gridMap.getResolution();
  position_ = gridMap.getPosition();
  size_ = gridMap.getSize();
  Matrix map = gridMap.get(layer); // Copy!

  float minHeight = map.minCoeffOfFinites();
  if (!std::isfinite(minHeight)) minHeight = lowestHeight_;
  float maxHeight = map.maxCoeffOfFinites();
  if (!std::isfinite(maxHeight)) maxHeight = lowestHeight_;

  const float valueForEmptyCells = lowestHeight_; // maxHeight, minHeight (TODO Make this an option).
  for (size_t i = 0; i < map.size(); ++i) {
    if (std::isnan(map(i))) map(i) = valueForEmptyCells;
  }

  // Height range of the signed distance field is higher than the max height.
  maxHeight += heightClearance;

  Matrix sdfElevationAbove = Matrix::Ones(map.rows(), map.cols()) * maxDistance_;
  Matrix sdfLayer = Matrix::Zero(map.rows(), map.cols());
  std::vector<Matrix> sdf;
  zIndexStartHeight_ = minHeight;

  // Calculate signed distance field from bottom.
  for (float h = minHeight; h < maxHeight; h += resolution_) {
    Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleFreeField = map.array() < h;
    Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> obstacleField = obstacleFreeField.array() < 1;
    Matrix sdfObstacle = getPlanarSignedDistanceField(obstacleField);
    Matrix sdfObstacleFree = getPlanarSignedDistanceField(obstacleFreeField);
    Matrix sdf2d;
    // If 2d sdfObstacleFree calculation failed, neglect this SDF
    // to avoid extreme small distances (-INF).
    if ((sdfObstacleFree.array() >= INF).any()) sdf2d = sdfObstacle;
    else sdf2d = sdfObstacle - sdfObstacleFree;
    sdf2d *= resolution_;
    for (size_t i = 0; i < sdfElevationAbove.size(); ++i) {
      if(sdfElevationAbove(i) == maxDistance_ && map(i) <= h) sdfElevationAbove(i) = h - map(i);
      else if(sdfElevationAbove(i) != maxDistance_ && map(i) <= h) sdfElevationAbove(i) = sdfLayer(i) + resolution_;
      if (sdf2d(i) == 0) sdfLayer(i) = h - map(i);
      else if (sdf2d(i) < 0) sdfLayer(i) = -std::min(fabs(sdf2d(i)), fabs(map(i) - h));
      else sdfLayer(i) = std::min(sdf2d(i), sdfElevationAbove(i));
    }
    data_.push_back(sdfLayer);
  }
}

  void inflateBlacklist(GridMap& map, std::set<tf::Vector3> positions, int radius)
  {
    CHECK_RADIUS(radius);

    static GridMask* grid_mask = NULL;
    static int last_radius = -1;
    if (!grid_mask || radius != last_radius)
    {
      delete grid_mask;
      grid_mask = makeCircularMask(radius);
      last_radius = radius;
    }

    ApplyMaskOperator op = [](int8_t cell, int8_t mask_cell)
    {
      // If the cell isn't unknown, set it as unreachable.
      if (cell == GridMap::UNKNOWN)
        return cell;
      else
        return std::max(cell, mask_cell);
    };

    for (tf::Vector3 pos : positions)
    {
      index_t idx = map.getIdxForPosition(pos);
      ROS_WARN_STREAM("Applying mask at idx=" << idx);
      apply_mask_at_idx(map, *grid_mask, op, idx);
    }
  }

	void Load()
	{

	  char filename[1024];
	  if ( LoadVMap ( mapid,gx,gy ) != VMAP::VMAP_LOAD_RESULT_OK )
	  {
		printf("Cannot load vmap: Map %d , GridX %d, GridY %d\n",mapid,gx,gy);
		return;
	  }
	  sprintf ( filename,"maps/%03u%02d%02d.map",mapid, gx,gy);
	  map = new GridMap;
	  cout << "Loading " << filename << endl;
	  bool r = map->loadData ( filename );
	  if ( !r )
	  {
		cout << "Failed to load map " << filename << endl;
		delete map;
		VMAP::VMapFactory::createOrGetVMapManager()->unloadMap(mapid,gx,gy);
		map = NULL;
		return;
	  }
	  loaded = true;
	  loadedmaps++;

	}

void BasicVFF::calcDistanceVecArray(const GridMap &map)
{
    for(int i = 0; i < map.getMapSize().y; i++)
    {
        for(int j = 0; j < map.getMapSize().x; j++)
        {
			if (map.getOccupancyGridMap()[i * map.getMapSize().x + j] > 0)
            {
                Position2f ObstaclePos;
                map.getPositionFromIndex(Cell2i(i,j), ObstaclePos);
                cout << i << " " << j << endl;
                cout << ObstaclePos.x << " " << ObstaclePos.y << endl;
                distance_vec_array.push_back(ObstaclePos);
                cout << distance_vec_array.size() << endl;

            }

        }
    }
}

int main(int argc,char ** argv)
{
GridMap * map;
map = new GridMap;
char filename[1024];
float x= atof(argv[1]);float y = atof(argv[2]);
sprintf(filename,"maps/%03u%02d%02d.map",1,(int)(32-x/SIZE_OF_GRIDS),(int)(32-y/SIZE_OF_GRIDS));
cout << "Loading " << filename << endl;
bool r = map->loadData(filename);
if (!r)
  cout << "Cannot load " << filename << " Replacing with blackmap" << endl;
else{
  printf("%f\n",map->getHeight(x,y));
  
    
  
  
  
}
}

MapTerrain2::MapTerrain2(uint32 MapId)
{
	m_MapId = MapId;
	memset( GridMaps, NULL, sizeof( GridMaps ) );
	for(uint32 y=0;y<MAX_NUMBER_OF_GRIDS;y++)
		for(uint32 x=0;x<MAX_NUMBER_OF_GRIDS;x++)
		{
			//gen filename
			char filename[500];
		    snprintf(filename, 500, "maps2/%03u%02u%02u.map",m_MapId,x,y);
			//see if file exist
		    FILE *pf=fopen(filename,"rb");
			if( pf == NULL )
				continue;
			fclose( pf );
			//load content into mem
			GridMap *gm = new GridMap();
			gm->loadData(filename);
			//store the struct 
			GridMaps[ x ] [ y ] = gm;
		}
}