//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;
}
