//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; }
uint32 MapTerrain2::GetAreaID(float x, float y, float z) { GridMap *g = GetGrid(x, y); if( g ) return g->getArea( x, y ); return 0; }
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; }
ZLiquidStatus MapTerrain2::GetLiquidStatus(float x, float y, float z) { GridMap *g = GetGrid(x, y); if( g ) { return g->getLiquidStatus( x, y, z, MAP_ALL_LIQUIDS, NULL ); } return LIQUID_MAP_NO_WATER; }
float MapTerrain2::GetLiquidHeight(float x, float y, float z) { GridMap *g = GetGrid(x, y); if( g ) { float level = g->getLiquidLevel( x, y ); if( level != INVALID_HEIGHT ) return level; } return VMAP_VALUE_NOT_INITIALIZED; }
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; }
uint8 MapTerrain2::GetLiquidFlags(float x, float y, float z) { GridMap *g = GetGrid(x, y); if( g ) { //check if we are above the liquid level (standing on bridge or flying) to avoid environmental dmg float level = g->getLiquidLevel( x, y ); if( level == INVALID_HEIGHT ) return 0; //seems like we are below (inside) liquid return g->getTerrainType( x, y ); } return 0; }
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++; }
EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation) : center_(center) { semiAxisSquare_ = (0.5 * length).square(); double sinRotation = sin(rotation); double cosRotation = cos(rotation); transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation; mapLength_ = gridMap.getLength(); mapPosition_ = gridMap.getPosition(); resolution_ = gridMap.getResolution(); bufferSize_ = gridMap.getSize(); bufferStartIndex_ = gridMap.getStartIndex(); Index submapStartIndex; Index submapBufferSize; findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize); internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize)); if(!isInside()) ++(*this); }
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; } }
void PotentialField::vscan_points_callback( sensor_msgs::PointCloud2::ConstPtr vscan_msg) { static VscanPointsFieldParamater param; double around_x(param.around_x); double around_y(param.around_y); ros::Time time = ros::Time::now(); pcl::PointCloud<pcl::PointXYZ> pcl_vscan; pcl::fromROSMsg(*vscan_msg, pcl_vscan); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_vscan_ptr( new pcl::PointCloud<pcl::PointXYZ>(pcl_vscan)); double length_x = map_.getLength().x() / 2.0; double length_y = map_.getLength().y() / 2.0; for (GridMapIterator it(map_); !it.isPastEnd(); ++it) { Position position; map_.getPosition(*it, position); map_.at("vscan_points_field", *it) = 0.0; for (int i(0); i < (int)pcl_vscan.size(); ++i) { double point_x = pcl_vscan.at(i).x - map_x_offset_; if (3.0 < pcl_vscan.at(i).z + tf_z_ || pcl_vscan.at(i).z + tf_z_ < 0.3) continue; if (length_x < point_x && point_x < -1.0 * length_x) continue; if (length_y < pcl_vscan.at(i).y && pcl_vscan.at(i).y < -1.0 * length_y) continue; if ((point_x + tf_x_) - around_x < position.x() && position.x() < point_x + tf_x_ + around_x) { if (pcl_vscan.at(i).y - around_y < position.y() && position.y() < pcl_vscan.at(i).y + around_y) { map_.at("vscan_points_field", *it) = 1.0; // std::exp(0.0) ; } } } } map_.setTimestamp(time.toNSec()); publish_potential_field(); }
float getheight(float x,float y,float z) { //printf("getheight(%f,%f,%f) MAP %d\n",x,y,z,mapid); if (!loaded ) { cout << "Getheight on non loaded vmap" << endl; return -50000; } float maph = map->getHeight ( x,y ); float vmaph = VMAP::VMapFactory::createOrGetVMapManager()->getHeight ( mapid, x, y, z + 5.0f ); maph = std::max ( maph,vmaph ); return maph; }
void goThroughPixels(cv::Mat& im_purp_contour, GridMap& local_map) { int num_rows = im_purp_contour.rows; int num_cols = im_purp_contour.cols;//FRAME_SIZE_X; for (int i=0; i<num_rows; i++) { // from bottom to top for (int j=0; j<num_cols; j++) { //std::cout<<i<<" "<<j; uchar tmp = im_purp_contour.at<uchar>(i,j); // get gray value if(tmp == COLOR_WHITE) { Eigen::Vector2d pt_im; pt_im << j, i; // col, row --> x, y Eigen::Vector2d pt_c = CameraMath::reconstructPoint2D(pt_im, 0); // on the floor int x = floor(30+pt_c[0]); // hacking for now int y = floor(30+pt_c[1]); local_map.setVal(x,y,255); } } } }
GridMap dilate(const GridMap& map, int amount) { const int w = map[0].size(), h = map.size(); SDL_Surface* surf = SDL_CreateRGBSurface(0, w, h, 32, 0, 0, 0xff, 0); for (int y = 0; y < h; y++) { for (int x = 0; x < w; x++) { if (map[y][x]) { filledCircleColor(surf, x, y, amount, 0xffffff); } } } GridMap out(h, GridRow(w)); for (int y = 0; y < h; y++) { Uint32* srcrow = reinterpret_cast<Uint32*>(static_cast<char*>(surf->pixels) + surf->pitch * y); GridRow& dstrow = out[y]; for (int x = 0; x < w; x++) { if (srcrow[x]) dstrow[x] = true; } } return out; }
void populateMap(GridMap &map, string layer,string file_path, float scale,float res) { cv_bridge::CvImage cv_image; Mat img = imread(file_path,CV_LOAD_IMAGE_GRAYSCALE); cv::Size img_size= img.size(); cv::resize(img,img,cv::Size(),scale,scale, CV_INTER_CUBIC); map.setGeometry(Length(img.rows*res,img.cols*res),res); for (GridMapIterator it(map); !it.isPastEnd(); ++it) { Position position; map.getPosition(*it, position); int x = (img.rows/2) + position.x()/res+res/2.0; int y = (img.cols/2) + position.y()/res+res/2.0; map.at(layer, *it) = img.at<uchar>(x,y)<200?1:0; } map.setTimestamp(ros::Time::now().toNSec()); ROS_INFO("Created map with size %f x %f m (%i x %i cells).",map.getLength().x(), map.getLength().y(),map.getSize()(0), map.getSize()(1)); }
void next_test() { enum {GRID_SIZE = 32}; // build map GridMap *gm = new GridMap(30, 30, GRID_SIZE); gm->setTarget(5 * GRID_SIZE, 5 * GRID_SIZE); for (int i = 0; i < 20; ++i) { gm->setWalkableAt(10 * GRID_SIZE, i * GRID_SIZE, false); } for (int i = 10; i < 20; ++i) { gm->setWalkableAt(i * GRID_SIZE, 10 * GRID_SIZE, false); } gm->updateRoute(); for (int i = 0; i < 30; ++i) { for (int j = 0; j < 30; ++j) { printf("%d,%d ", gm->getNextX(j * 32 + 16, i * 32 + 16), gm->getNextY(j * 32 + 16, i * 32 + 16)); } puts(""); } }
void PotentialField::init() { ROS_INFO("Created map"); map_.setFrameId("/potential_field_link"); map_.setGeometry(Length(map_x_size_, map_y_size_), map_resolution_); for (GridMapIterator it(map_); !it.isPastEnd(); ++it) { Position position; map_.getPosition(*it, position); map_.at("obstacle_field", *it) = 0.0; map_.at("target_waypoint_field", *it) = 0.0; map_.at("vscan_points_field", *it) = 0.0; map_.at("potential_field", *it) = 0.0; } ROS_INFO("Created map with size %f x %f m (%i x %i cells).", map_.getLength().x(), map_.getLength().y(), map_.getSize()(0), map_.getSize()(1)); }
void PotentialField::obj_callback( autoware_msgs::DetectedObjectArray::ConstPtr obj_msg) { // Create grid map. static ObstacleFieldParameter param; double ver_x_p(param.ver_x_p); double ver_y_p(param.ver_y_p); // Add data to grid map. ros::Time time = ros::Time::now(); for (GridMapIterator it(map_); !it.isPastEnd(); ++it) { Position position; map_.getPosition(*it, position); map_.at("obstacle_field", *it) = 0.0; for (int i(0); i < (int)obj_msg->objects.size(); ++i) { double pos_x = obj_msg->objects.at(i).pose.position.x + tf_x_ - map_x_offset_; double pos_y = obj_msg->objects.at(i).pose.position.y; double len_x = obj_msg->objects.at(i).dimensions.x / 2.0; double len_y = obj_msg->objects.at(i).dimensions.y / 2.0; double r, p, y; tf::Quaternion quat(obj_msg->objects.at(i).pose.orientation.x, obj_msg->objects.at(i).pose.orientation.y, obj_msg->objects.at(i).pose.orientation.z, obj_msg->objects.at(i).pose.orientation.w); tf::Matrix3x3(quat).getRPY(r, p, y); double rotated_pos_x = std::cos(-1.0 * y) * (position.x() - pos_x) - std::sin(-1.0 * y) * (position.y() - pos_y) + pos_x; double rotated_pos_y = std::sin(-1.0 * y) * (position.x() - pos_x) + std::cos(-1.0 * y) * (position.y() - pos_y) + pos_y; if (-0.5 < pos_x && pos_x < 4.0) { if (-1.0 < pos_y && pos_y < 1.0) continue; } if (pos_x - len_x < rotated_pos_x && rotated_pos_x < pos_x + len_x) { if (pos_y - len_y < rotated_pos_y && rotated_pos_y < pos_y + len_y) { map_.at("obstacle_field", *it) = std::max(std::exp(0.0), static_cast<double>(map_.at("obstacle_field", *it))); } else if (rotated_pos_y < pos_y - len_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_y - (pos_y - len_y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } else if (pos_y + len_y < rotated_pos_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_y - (pos_y + len_y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } } else if (rotated_pos_x < pos_x - len_x) { if (rotated_pos_y < pos_y - len_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_y - (pos_y - len_y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0))) + (-1.0 * (std::pow((rotated_pos_x - (pos_x - len_x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } else if (pos_y + len_y < rotated_pos_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_y - (pos_y + len_y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0))) + (-1.0 * (std::pow((rotated_pos_x - (pos_x - len_x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } else if (pos_y - len_y < rotated_pos_y && rotated_pos_y < pos_y + len_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_x - (pos_x - len_x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } } else if (pos_x + len_x < rotated_pos_x) { if (rotated_pos_y < pos_y - len_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_y - (pos_y - len_y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0))) + (-1.0 * (std::pow((rotated_pos_x - (pos_x + len_x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } else if (pos_y + len_y / 2.0 < rotated_pos_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_y - (pos_y + len_y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0))) + (-1.0 * (std::pow((rotated_pos_x - (pos_x + len_x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } else if (pos_y - len_y < rotated_pos_y && rotated_pos_y < pos_y + len_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_x - (pos_x + len_x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } } } } // Publish grid map. map_.setTimestamp(time.toNSec()); publish_potential_field(); }
TEST(GridMap, Move) { GridMap map; map.setGeometry(Length(8.1, 5.1), 1.0, Position(0.0, 0.0)); // bufferSize(8, 5) map.add("layer", 0.0); map.setBasicLayers(map.getLayers()); std::vector<BufferRegion> regions; map.move(Position(-3.0, -2.0), regions); Index startIndex = map.getStartIndex(); EXPECT_EQ(3, startIndex(0)); EXPECT_EQ(2, startIndex(1)); EXPECT_FALSE(map.isValid(Index(0, 0))); // TODO Check entire map. EXPECT_TRUE(map.isValid(Index(3, 2))); EXPECT_FALSE(map.isValid(Index(2, 2))); EXPECT_FALSE(map.isValid(Index(3, 1))); EXPECT_TRUE(map.isValid(Index(7, 4))); EXPECT_EQ(2, regions.size()); EXPECT_EQ(0, regions[0].getStartIndex()[0]); EXPECT_EQ(0, regions[0].getStartIndex()[1]); EXPECT_EQ(3, regions[0].getSize()[0]); EXPECT_EQ(5, regions[0].getSize()[1]); EXPECT_EQ(0, regions[1].getStartIndex()[0]); EXPECT_EQ(0, regions[1].getStartIndex()[1]); EXPECT_EQ(8, regions[1].getSize()[0]); EXPECT_EQ(2, regions[1].getSize()[1]); }
double PlannerH::PlanUsingReedSheppWithObstacleDetection(const WayPoint& start, const WayPoint& goal,GridMap& map, vector<WayPoint>& genSmoothedPath, const double pathDensity , const double smoothFactor ) { RSPlanner rs_planner(smoothFactor); int numero = 0; double t=0, u=0 , v=0; rs_planner.PATHDENSITY = pathDensity; genSmoothedPath.clear(); genSmoothedPath.clear(); double length = rs_planner.min_length_rs(start.pos.x, start.pos.y, UtilityHNS::UtilityH::SplitPositiveAngle(start.pos.a), goal.pos.x, goal.pos.y, UtilityHNS::UtilityH::SplitPositiveAngle(goal.pos.a), numero, t , u , v); rs_planner.constRS(numero, t, u , v, start.pos.x, start.pos.y, UtilityHNS::UtilityH::SplitPositiveAngle(start.pos.a), rs_planner.PATHDENSITY, genSmoothedPath); if(genSmoothedPath.size() == 0) return length; CELL_Info* pCellRet = 0; WayPoint p = genSmoothedPath.at(0); int nChanges = 0; double nMinChangeDistance = length; double d = 0; for(unsigned int i=0; i<genSmoothedPath.size(); i++) { if(p.bDir != genSmoothedPath.at(i).bDir) { if(d < nMinChangeDistance) nMinChangeDistance = d; d = 0; nChanges++; } d+= distance2points(p.pos, genSmoothedPath.at(i).pos); p = genSmoothedPath.at(i); // if(map.) // pCellRet = map.GetCellFromPointInnerMap(p.p); // else pCellRet = map.GetCellFromPoint(POINT2D(p.pos.x, p.pos.y)); if(pCellRet) { if(pCellRet->nMovingPoints > 0|| pCellRet->nStaticPoints > 0 || pCellRet->heuristic == map.m_MaxHeuristics) { cout << "\n Obstacle Detected \n"; genSmoothedPath.clear(); return -1; } } else { cout << "\n Outside the Main Grid \n"; genSmoothedPath.clear(); return -1; } } if(nChanges > 3 || nMinChangeDistance < 3.2) { cout << "\n Too much gear changes \n"; genSmoothedPath.clear(); return -1; } // pthread_mutex_lock(&planning_mutex); // m_CurrentPath.assign(genSmoothedPath.begin(), genSmoothedPath.end()); // m_CurrentSmoothPath.clear(); // //m_TotalPath.assign(m_CurrentPath.begin(), m_CurrentPath.end()); // m_CurrentSmoothPath.assign(m_CurrentPath.begin(), m_CurrentPath.end()); // //m_TotalSmoothPath.assign(m_CurrentSmoothPath.begin(), m_CurrentSmoothPath.end()); // pthread_mutex_unlock(&planning_mutex); return length; }