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 ); } } } } }
double getDist(double x, double y) { int v = getMapValue(*mapDistance, x, y); if(v < 0) return -10.0; return (v * mapDistance->info.resolution); }
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(); }
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); } }
// 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; }
// 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); }
// 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(); }
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; }
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); }