local_ref<jstring> ReadableNativeMap::getStringKey(const std::string& key) {
  const folly::dynamic& val = getMapValue(key);
  if (val.isNull()) {
    return local_ref<jstring>(nullptr);
  }
  return make_jstring(val.getString().c_str());
}
void BarycentricMapperMeshTopology<CudaVec3fTypes,CudaVec3fTypes>::applyJT( In::MatrixDeriv& out, const Out::MatrixDeriv& in)
{

    Out::MatrixDeriv::RowConstIterator rowItEnd = in.end();

    for ( Out::MatrixDeriv::RowConstIterator rowIt = in.begin(); rowIt != rowItEnd; ++rowIt)
    {
        Out::MatrixDeriv::ColConstIterator colItEnd = rowIt.end();
        Out::MatrixDeriv::ColConstIterator colIt = rowIt.begin();

        int indexIn;

        if (colIt != colItEnd)
        {
            In::MatrixDeriv::RowIterator o = out.writeLine(rowIt.index());
            for ( ; colIt != colItEnd; ++colIt)
            {
                indexIn = colIt.index();
                InDeriv data = (InDeriv) colIt.val();

                for (int j=0; j<maxNIn; ++j)
                {
                    const OutReal f = ( OutReal ) getMapValue(indexIn,j);
                    int index = getMapIndex(indexIn,j);
                    if (index < 0) break;
                    o.addCol( index, data * f );
                }

            }
        }
    }
}
Esempio n. 3
0
double getDist(double x, double y)
{
    int v = getMapValue(*mapDistance, x, y);
    if(v < 0)
        return -10.0;
    return (v * mapDistance->info.resolution);
}
Esempio n. 4
0
string Object::getValue()
{
    if (isArray == 1)
    {
        getMapValue();
    }
    
    if (value=="") return "";
    
    if (type=="bool")
    {
        return to_string(getBoolValue());
    }
    else if (type=="int")
    {
        return to_string(getIntValue());
    }
    else if (type=="double")
    {
        std::ostringstream os;
        os << getDoubleValue();
        return os.str();
    }
    return getStringValue();
}
double ReadableNativeMap::getDoubleKey(const std::string& key) {
  const folly::dynamic& val = getMapValue(key);
  if (val.isInt()) {
    return val.getInt();
  }
  return val.getDouble();
}
Esempio n. 6
0
bool isPointFree(double x, double y)
{
    int v = getMapValue(*mapInflated, x, y);
    if(v < 0)
        return false;
    return (v < minOccupied);
}
local_ref<ReadableNativeArray::jhybridobject> ReadableNativeMap::getArrayKey(const std::string& key) {
  auto& value = getMapValue(key);
  if (value.isNull()) {
    return local_ref<ReadableNativeArray::jhybridobject>(nullptr);
  } else {
    return ReadableNativeArray::newObjectCxxArgs(value);
  }
}
local_ref<ReadableNativeMap::jhybridobject> ReadableNativeMap::getMapKey(const std::string& key) {
  auto& value = getMapValue(key);
  if (value.isNull()) {
    return local_ref<ReadableNativeMap::jhybridobject>(nullptr);
  } else if (!value.isObject()) {
    throwNewJavaException(exceptions::gUnexpectedNativeTypeExceptionClass,
                          "expected Map, got a %s", value.typeName());
  } else {
    return ReadableNativeMap::newObjectCxxArgs(value);
  }
}
Esempio n. 9
0
// Prints the map to console
void printMap()
{
    for (int x = 0; x < row; ++x) {
        for (int y = 0; y < col; ++y) {
            // Prints the value at current x,y location
            std::cout << getMapValue(map[x][y]);
        }
        // Ends the line for next x value
        std::cout << std::endl;
    }
}
 bool isFree(std::vector<int> cells, const nav_msgs::OccupancyGrid& occupancyGrid) {
     for (int i = 0; i < cells.size(); i += 2) {
         std::vector<int> current;
         current.push_back(cells[i]);
         current.push_back(cells[i + 1]);
         if (getMapValue(occupancyGrid, current[0], current[1]) <=0) {
             return false;
         }
     }
     return true;
 }
Esempio n. 11
0
// Checks, whether the given coordinate is a frontier point or not.
// Needs to take care of the actual positions, corners and borders
// need to be handled carefully
bool isFrontier(const nav_msgs::OccupancyGrid& grid, int x, int y) {
    std::vector<int> neighbors = getNeighbors(grid, x, y);
    int numExplored = 0;
    int numUnknown = 0;

    // Checking neighbors
    // If there are known and unknown neighbors
    // the robot is at a frontier
    bool obstacle = false;
    for (unsigned int i = 0; i < neighbors.size(); i += 2) {
        std::vector<int> current;
        current.push_back(neighbors[i]);
        current.push_back(neighbors[i + 1]);
        if (getMapValue(grid, current[0], current[1]) < 0)
            numUnknown++;
        else if (getMapValue(grid, current[0], current[1]) > 50 )
            obstacle = true;
        else
            numExplored++;
    }

    return(numExplored >= (neighbors.size()/4) && numUnknown >= (neighbors.size()/4) && !obstacle);
}
Esempio n. 12
0
// Algorithm for Wavefront Frontier Detection
std::vector<std::vector<int> > frontierDetection(const nav_msgs::OccupancyGrid& map, int x, int y) {

    std::vector<std::vector<int> > frontierList;
    std::vector<int> pose;
    std::vector<std::vector<int> > marker;

    for(unsigned int i = 0 ; i < map.info.width ; i++) {
        std::vector<int> row;
        for(unsigned int j = 0 ; j < map.info.height ; j++) {
            row.push_back(-1);
        }
        marker.push_back(row);
    }

    pose.push_back(x); // x-coordinate robot
    pose.push_back(y); // y-coordinate robot

    // Create neccessary lists
    std::vector<std::vector<int> > queue_m;
    queue_m.push_back(pose);

    int index1 = pose[0];
    int index2 = pose[1];

    marker[index1][index2] = MAP_open_list;

    while (!queue_m.empty()) {

        // Dequeue
        std::vector<int> p = queue_m.front();
        queue_m.erase(queue_m.begin());

        // If map close list contains p, continue
        index1 = p[0];
        index2 = p[1];
        if(marker[index1][index2] == MAP_close_list)
            continue;

        if (isFrontier(map, p[0], p[1])) {
            std::vector<std::vector<int> > queue_f;
            std::vector<int> newFrontier;
            queue_f.push_back(p);
            marker[index1][index2] = FRONTIER_open_list;

            while (!queue_f.empty()) {
                std::vector<int> q = queue_f.front();
                queue_f.erase(queue_f.begin());
                index1 = q[0];
                index2 = q[1];

                if(marker[index1][index2] == MAP_close_list || marker[index1][index2] == FRONTIER_close_list)
                    continue;

                if (isFrontier(map, q[0], q[1])) {
                    newFrontier.push_back(q[0]);
                    newFrontier.push_back(q[1]);
                    std::vector<int> neighbors = getNeighbors(map, q[0], q[1]);
                    for (unsigned int i = 0; i < neighbors.size(); i += 2) {
                        std::vector<int> w;
                        w.push_back(neighbors[i]);
                        w.push_back(neighbors[i + 1]);
                        index1 = w[0];
                        index2 = w[1];
                        if(marker[index1][index2] != FRONTIER_open_list
                                && marker[index1][index2] != FRONTIER_close_list
                                && marker[index1][index2] != MAP_close_list) {
                            queue_f.push_back(w);
                            marker[index1][index2] = FRONTIER_open_list;
                        }
                    }
                }
                index1 = q[0];
                index2 = q[1];
                marker[index1][index2] = FRONTIER_close_list;
            }

            // Save data of newFrontier and mark points as map_close_list
            frontierList.push_back(newFrontier);

            for (unsigned int i = 0; i < newFrontier.size(); i++) {
                index1 = newFrontier[i];
                index2 = newFrontier[i+1];
                marker[index1][index2] = MAP_close_list;
                i++;
            }
        }

        std::vector<int> neighbors = getNeighbors(map, p[0], p[1]);

        for (unsigned int i = 0; i < neighbors.size(); i += 2) {
            std::vector<int> v;
            v.push_back(neighbors[i]);
            v.push_back(neighbors[i + 1]);
            index1 = v[0];
            index2 = v[1];
            if(marker[index1][index2] != MAP_open_list
                    && marker[index1][index2] != MAP_close_list) {

                std::vector<int> neighborsV = getNeighbors(map, v[0], v[1]);

                bool hasOpenSpaceNeighbor = false;

                for (unsigned int j = 0; j < neighborsV.size(); j += 2) {

                    std::vector<int> neighborV;
                    neighborV.push_back(neighborsV[j]);
                    neighborV.push_back(neighborsV[j + 1]);

                    int mapVal = getMapValue(map, neighborV[0], neighborV[1]);

                    if (mapVal < 10 && mapVal != -1) {
                        hasOpenSpaceNeighbor = true;
                        break;
                    }
                }

                if (hasOpenSpaceNeighbor) {
                    queue_m.push_back(v);
                    index1 = v[0];
                    index2 = v[1];
                    marker[index1][index2] = MAP_open_list;
                }
            }
        }
        index1 = p[0];
        index2 = p[1];
        marker[index1][index2] = MAP_close_list;

    }

    return frontierList;
}
bool ReadableNativeMap::isNull(const std::string& key) {
  return getMapValue(key).isNull();
}
Esempio n. 14
0
int main(int argc, char **argv) {
  // int mapSize = 100;

  lua_State *L = luaL_newstate();
  luaL_openlibs(L);
  luaL_dofile(L, "island.lua");

  lua_getglobal(L, "getMapSize");
  lua_pcall(L, 0, 1, 0);
  int mapSize = (int)lua_tointeger(L, -1);
  int tileSize = 5;
  int tileSizeX = 640 / mapSize;
  int tileSizeY = 480 / mapSize;

  //do lua stuff
  if (SDL_Init(SDL_INIT_VIDEO|SDL_INIT_TIMER) != 0) {
    fprintf(stderr, "\nUnable to initialize SDL:  %s\n", SDL_GetError());
    return 1;
  }
  
  atexit(SDL_Quit);

  SDL_Window *window = SDL_CreateWindow("Surveil", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, 640, 480, SDL_WINDOW_SHOWN);
  SDL_Renderer *context = SDL_CreateRenderer(window, -1, SDL_RENDERER_ACCELERATED);

  if(context == NULL) {
    fprintf(stderr, "Could not creat rendering context: %s\n", SDL_GetError());
    printf("Falling back on software renderer . . .\n");
    context = SDL_CreateRenderer(window, -1, SDL_RENDERER_SOFTWARE);
    if(context == NULL) {
      fprintf(stderr, "Could not initialize software renderer. Well this is embarrasing . . .%s\n", SDL_GetError());
    }
  }

  if(window == NULL) {
    fprintf(stderr, "\nCould not create SDL Window %s\n", SDL_GetError());
  }

  bool isRunning = true;

  while(isRunning) {
    SDL_Event event;

    while(SDL_PollEvent(&event) != 0) {
      if(event.type == SDL_QUIT) {
        isRunning = false;
      }
    }

    SDL_SetRenderDrawColor(context, 0xFF, 0xFF, 0xFF, 0xFF);
    SDL_RenderClear(context);

    int x, y;
    for(y=0; y<mapSize; y++) {
      for(x=0; x<mapSize; x++) {
        SDL_Rect tile = {x*tileSizeX, y*tileSizeY, tileSizeX, tileSizeY};
        int val = getMapValue(L, x, y);

        switch(val) {
          case 0: {//Water
            SDL_SetRenderDrawColor(context, 0x00, 0x00, 0xFF, 0xFF);
            break;
          }
          case 1: {//Grass
            SDL_SetRenderDrawColor(context, 0x00, 0xFF, 0x00, 0xFF);
            break;
          }
          case 3: {//Snow
            SDL_SetRenderDrawColor(context, 0xFF, 0xFF, 0xFF, 0xFF);
            break;
          }
          case 2: {//Rock
            SDL_SetRenderDrawColor(context, 0x88, 0x88, 0x88, 0xFF);
            break;
          }
          case 4: {//Forest
            SDL_SetRenderDrawColor(context, 0x00, 0x88, 0x00, 0xFF);
            break;
          }
          case 5: {//sand
            SDL_SetRenderDrawColor(context, 0xED, 0xC9, 0xAF, 0xFF);
            break;
          }
          case 6: {//Foam
            SDL_SetRenderDrawColor(context, 0x35, 0x35, 0xFF, 0xFF);
            break;
          }
        }
        SDL_RenderFillRect(context, &tile);
      }
    }

    SDL_RenderPresent(context);
  }
    

  lua_close(L);
  SDL_Quit();

  return 0;
}
Esempio n. 15
0
XBool XSlider::mouseProc(const XVec2& p,XMouseState mouseState)
{
	m_upMousePoint.set(p);
	if(!m_isInited ||	//如果没有初始化直接退出
		!m_isActive ||		//没有激活的控件不接收控制
		!m_isVisible ||	//如果不可见直接退出
		!m_isEnable) return XFalse;		//如果无效则直接退出
	if(m_withAction && m_isInAction) return XFalse;	//如果支持动作播放而且正在播放动画那么不接受鼠标控制
	if(m_isSilent) return XFalse;
	//SliderLine,滚动条的线的事件

	if(m_curSliderState == SLIDER_STATE_DOWN)
	{//注意这里是滑动条拖动时,为了是超出范围的拖动仍然有效,所以这个这个特殊处理
		XRect tempFly(m_curMouseRect.getLT() - m_mouseFlyArea, m_curMouseRect.getRB() + m_mouseFlyArea);
		if(tempFly.isInRect(p))
		{//拖动(注意如果这里拖动超出范围,则使用最后一次的有效值)
			switch(mouseState)
			{
			case MOUSE_MOVE://重新计算Slider的当前值
				if(m_typeVorH == SLIDER_TYPE_VERTICAL) m_curValue = getMapValue(p.y);else
				if(m_typeVorH == SLIDER_TYPE_HORIZONTAL) m_curValue = getMapValue(p.x);
				updateButtonData();
				m_dataChanged = XTrue;	//标记数值发生改变
				if(m_eventProc != NULL) m_eventProc(m_pClass,m_objectID,SLD_MOUSE_MOVE);
				else XCtrlManager.eventProc(m_objectID,SLD_MOUSE_MOVE);
				break;
			case MOUSE_LEFT_BUTTON_UP://这个弹起事件是在按钮上按下的弹起事件
				m_curSliderState = SLIDER_STATE_ON;
				if(m_eventProc != NULL) m_eventProc(m_pClass,m_objectID,SLD_MOUSE_UP);
				else XCtrlManager.eventProc(m_objectID,SLD_MOUSE_UP);
				if(m_dataChanged)
				{
					stateChange();
					if(m_eventProc != NULL)
						m_eventProc(m_pClass,m_objectID,SLD_VALUE_CHANGE);
					else
						XCtrlManager.eventProc(m_objectID,SLD_VALUE_CHANGE);
					m_dataChanged = XFalse;
				}
				break;
			}
			m_isBeChoose = XTrue;
		}else
		{
			m_curSliderState = SLIDER_STATE_NORMAL;
			if(m_dataChanged)
			{
				stateChange();
				if(m_eventProc != NULL)
					m_eventProc(m_pClass,m_objectID,SLD_VALUE_CHANGE);
				else
					XCtrlManager.eventProc(m_objectID,SLD_VALUE_CHANGE);
				m_dataChanged = XFalse;
			}
		}
	}
	if(m_curMouseRect.isInRect(p))
	{//鼠标动作在范围内
		switch(mouseState)
		{
		case MOUSE_MOVE:
			if(m_curSliderState != SLIDER_STATE_NORMAL) break; //悬浮
			m_curSliderState = SLIDER_STATE_ON;
			if(m_eventProc != NULL) m_eventProc(m_pClass,m_objectID,SLD_MOUSE_ON);
			else XCtrlManager.eventProc(m_objectID,SLD_MOUSE_ON);
			if(m_withAction)
			{//这里测试一个动态效果
				m_isInAction = XTrue;
				m_actionMoveData.set(1.0f,1.1f,0.02f,MD_MODE_SIN_MULT,1);
				m_lightMD.set(1.0f,2.0f,0.002f,MD_MODE_SIN_MULT);
				m_oldPos = m_position;
				m_oldSize = m_scale;
			} 
			break;
		case MOUSE_LEFT_BUTTON_DOWN:
		case MOUSE_LEFT_BUTTON_DCLICK:
			if(m_curSliderState != SLIDER_STATE_NORMAL && m_curSliderState != SLIDER_STATE_ON) break;
			m_upValue = m_curValue;

			//重新计算Slider的当前值
			if(m_typeVorH == SLIDER_TYPE_VERTICAL) m_curValue = getMapValue(p.y);else
			if(m_typeVorH == SLIDER_TYPE_HORIZONTAL) m_curValue = getMapValue(p.x);

			updateButtonData();
			m_dataChanged = XTrue;	//标记数值发生改变
			m_curSliderState = SLIDER_STATE_DOWN;
			if(m_eventProc != NULL) m_eventProc(m_pClass,m_objectID,SLD_MOUSE_DOWN);
			else XCtrlManager.eventProc(m_objectID,SLD_MOUSE_DOWN);
			break;
		case MOUSE_LEFT_BUTTON_UP:
			if(m_curSliderState != SLIDER_STATE_DOWN) break;
			//这里会触发数值改变事件(这个弹起事件应该是在线上按下的弹起)(*这里貌似不会发生*)
			m_curSliderState = SLIDER_STATE_ON;
			if(m_eventProc != NULL) m_eventProc(m_pClass,m_objectID,SLD_MOUSE_UP);
			else XCtrlManager.eventProc(m_objectID,SLD_MOUSE_UP);
			if(m_dataChanged)
			{
				stateChange();
				if(m_eventProc != NULL)
					m_eventProc(m_pClass,m_objectID,SLD_VALUE_CHANGE);
				else
					XCtrlManager.eventProc(m_objectID,SLD_VALUE_CHANGE);
				m_dataChanged = XFalse;
			}
			break;
		case MOUSE_WHEEL_UP_DOWN:
			if(m_curSliderState != SLIDER_STATE_ON) break;
			if(m_curValue >= m_maxValue) break;
			m_curValue += m_keyOneValue * 0.1f;
			if(m_curValue >= m_maxValue) m_curValue = m_maxValue;
			updateButtonData();
			stateChange();
			if(m_eventProc != NULL)
				m_eventProc(m_pClass,m_objectID,SLD_VALUE_CHANGE);
			else
				XCtrlManager.eventProc(m_objectID,SLD_VALUE_CHANGE);
			m_dataChanged = XFalse;
			break;
		case MOUSE_WHEEL_DOWN_DOWN:
			if(m_curSliderState != SLIDER_STATE_ON) break;
			if(m_curValue <= m_minValue) break;
			m_curValue -= m_keyOneValue * 0.1f;
			if(m_curValue <= m_minValue) m_curValue = m_minValue;
			updateButtonData();
			stateChange();
			if(m_eventProc != NULL)
				m_eventProc(m_pClass,m_objectID,SLD_VALUE_CHANGE);
			else
				XCtrlManager.eventProc(m_objectID,SLD_VALUE_CHANGE);
			m_dataChanged = XFalse; 
			break;
		}
		if (!m_isMouseInRect)
		{
			m_isMouseInRect = XTrue;
			m_comment.setShow();
			setCommentPos(p + XVec2(0.0f, 16.0f));
		}
		if (mouseState != MOUSE_MOVE && m_comment.getIsShow())
			m_comment.disShow();	//鼠标的任意操作都会让说明框消失
	}else
	{
		if(m_curSliderState == SLIDER_STATE_ON) m_curSliderState = SLIDER_STATE_NORMAL;
		if (m_isMouseInRect)
		{
			m_isMouseInRect = XFalse;
			m_comment.disShow();
		}
	}
	return XTrue;
}
    void occupancyGridCallback(const nav_msgs::OccupancyGrid occupancyGrid) {
        if(!onTheMove) {
            onTheMove = true;
            grid = occupancyGrid;
            float resolution = occupancyGrid.info.resolution;
            tf::TransformListener listener(ros::Duration(10));
            //transform object storing our robot's position
            tf::StampedTransform transform;
            try {
                ros::Time now = ros::Time::now();
                geometry_msgs::PointStamped base_point;
                listener.waitForTransform("/world", "/base_link", now, ros::Duration(0.5));
                listener.lookupTransform("/world", "/base_link", ros::Time(0), transform);
                double x = transform.getOrigin().x();
                double y = transform.getOrigin().y();

//                ROS_INFO_STREAM("STARTPOSITION X: " <<x<<" STARTPOSITION Y: " <<y);
                robot_pos[0] = (int) ((x / resolution) + (int)(occupancyGrid.info.width/2));
                robot_pos[1] = (int) ((y / resolution) + (int)(occupancyGrid.info.height/2));
//                ROS_INFO("X = %i , Y = %i", robot_pos[0], robot_pos[1]);
//                ROS_INFO("LETS GET THE FRONTIERS");
                std::vector<std::vector<int> > frontiersList = frontierDetection(occupancyGrid, robot_pos[0], robot_pos[1]);
//                ROS_INFO("FRONIERS SERVED!");

                x = robotX;
                y = robotY;

                int num_frontier_cells = 0;

                for(unsigned int i = 0 ; i < frontiersList.size() ; i++ ) {
                    for(unsigned int j = 0 ; j < frontiersList[i].size() ; j++) {
                        num_frontier_cells++;
                    }
                }

//                ROS_INFO("%i frontier cells found",num_frontier_cells);
                double goalX = 0;
                double goalY = 0;
                frontier_cloud.points.resize(0);
                frontier_cloud.points.resize(num_frontier_cells);
                int frontierIndex = 0;
                //arbitrary value which is always higher than any distance found
                double minDist = 40000.0;
                //represents the closest frontier
                std::vector <int> closestFrontier;

                int fewestObstacles = 999999;

//                std::vector<std::vector<int> > blocked;

                //fill frontier cloud for publishing and calculate frontier closest to robot base
                for(unsigned int i = 0 ; i < frontiersList.size() ; i++ ) {
                    for(unsigned int j = 0 ; j < frontiersList[i].size() ; j++) {
                        double fX = (frontiersList[i][j] - (int)(occupancyGrid.info.width/2)) * occupancyGrid.info.resolution;
                        frontier_cloud.points[frontierIndex].x = fX;
                        double fY = (frontiersList[i][j+1] - (int)(occupancyGrid.info.height/2)) * occupancyGrid.info.resolution;


                        frontier_cloud.points[frontierIndex].y = fY;
                        frontier_cloud.points[frontierIndex].z = 0;
                        frontierIndex++;
                        j++;



                        if(!isBlocked(fX, fY))
                        {
                        double gridX = (fX / occupancyGrid.info.resolution) + (int)(occupancyGrid.info.width/2);
                        double gridY = (fY / occupancyGrid.info.resolution) + (int)(occupancyGrid.info.height/2);

                        // Get neighbors
                        std::vector<int> neighbors = getSurrounding(occupancyGrid, gridX, gridY, 20);

                        // Count obstacles
                        int obstacles = 0;
                        for (int a = 0; a < neighbors.size(); a += 2) {
                            std::vector<int> current;
                            current.push_back(neighbors[a]);
                            current.push_back(neighbors[a + 1]);
                            if (getMapValue(occupancyGrid, current[0], current[1]) > 0) {
                                obstacles++;
                            }
                        }

                        double distanceToRobot = sqrt((fX-x)*(fX-x)+(fY-y)*(fY-y));
                        double distanceToLastGoal = 5*sqrt((fX-prevGoalX)*(fX-prevGoalX)+(fY-prevGoalY)*(fY-prevGoalY));


                        obstacles += distanceToRobot+distanceToLastGoal;

                        if(obstacles < fewestObstacles)
                        {
                            fewestObstacles = obstacles;
                            double randomX = 1./(rand() % 100 + 1);
                            double randomY = 1./(rand() % 100 + 1);
                            double randomRange = 1;
                            goalX = fX;//-randomRange/2+randomRange*randomX;
                            goalY = fY;//-randomRange/2+randomRange*randomY;

                            std::cout << "distanceToRobot = " << distanceToRobot << std::endl;
                            std::cout << "distanceToLastGoal = " << distanceToLastGoal << std::endl;
                            std::cout << "obstacles = " << obstacles << std::endl;
                        }

//                        if(distance < minDist && distance > 2) {
//                            closestFrontier = frontiersList[i];
//                            minDist = distance;
//                            goalX = fX;
//                            goalY = fY;
//                        }
                        }
                    }
                }
                if(goalX == prevGoalX && goalY == prevGoalY)
                    sameCounter++;
                else
                    sameCounter = 0;
                if(sameCounter > 0)
                {
                    std::vector<double> error;
                    error.push_back(goalX);
                    error.push_back(goalY);
                    blocked.push_back(error);
                }
                prevGoalX = goalX;
                prevGoalY = goalY;
//                ROS_INFO_STREAM();
                // find a reachable goal position
                // select a target cell surrounded by free space only
//                double gridX = (goalX / occupancyGrid.info.resolution) + (int)(occupancyGrid.info.width/2);
//                double gridY = (goalY / occupancyGrid.info.resolution) + (int)(occupancyGrid.info.height/2);
//                std::vector<int> neighbors = getSurrounding(occupancyGrid, gridX, gridY, 50);
//                if(!isFree(neighbors, occupancyGrid)) {
//                    for (int i = 0; i < neighbors.size(); i += 2) {
//                        std::vector<int> current;
//                        current.push_back(neighbors[i]);
//                        current.push_back(neighbors[i + 1]);
//                        std::vector<int> currentNeighbors = getSurrounding(occupancyGrid, current[0], current[1], 10);
//                        if(isFree(currentNeighbors, occupancyGrid)) {
//                            goalX = (current[0] - (int)(occupancyGrid.info.width/2)) * occupancyGrid.info.resolution;
//                            goalY = (current[1] - (int)(occupancyGrid.info.width/2)) * occupancyGrid.info.resolution;
//                            break;
//                        }
//                    }
//                }

                next_frontier.points.resize(1);
                next_frontier.points[0].x = goalX;
                next_frontier.points[0].y = goalY;
                next_frontier.points[0].z = 0;

                frontier_publisher.publish(frontier_cloud);
                next_frontier_publisher.publish(next_frontier);
//                ROS_INFO("published cloud!");
                run(goalX,goalY);

            } catch (tf::TransformException& ex) {
                ROS_ERROR(
                            "Received an exception trying to transform a point from \"map\" to \"odom\": %s",
                            ex.what());
            }
        }
    }
local_ref<ReadableType> ReadableNativeMap::getValueType(const std::string& key) {
  return ReadableType::getType(getMapValue(key).type());
}
bool ReadableNativeMap::getBooleanKey(const std::string& key) {
  return getMapValue(key).getBool();
}
jint ReadableNativeMap::getIntKey(const std::string& key) {
  const folly::dynamic& val = getMapValue(key);
  int64_t integer = convertDynamicIfIntegral(val);
  return makeJIntOrThrow(integer);
}