void CPTG_RobotShape_Polygonal::setRobotShape(
const mrpt::math::CPolygon& robotShape)
{
ASSERT_ABOVEEQ_(robotShape.size(), 3u);
m_robotShape = robotShape;
m_robotMaxRadius = .0; // Default minimum
for (const auto& v : m_robotShape)
mrpt::utils::keep_max(m_robotMaxRadius, v.norm());
internal_processNewRobotShape();
}
/*---------------------------------------------------------------
loadFromFile
---------------------------------------------------------------*/
bool CParameterizedTrajectoryGenerator::CColisionGrid::loadFromFile( CStream *f, const mrpt::math::CPolygon & current_robotShape )
{
try
{
if (!f) return false;
// Return false if the file contents doesn't match what we expected:
uint32_t file_magic;
*f >> file_magic;
// It doesn't seem to be a valid file or was in an old format, just recompute the grid:
if (COLGRID_FILE_MAGIC!=file_magic)
return false;
uint8_t serialized_version;
*f >> serialized_version;
switch (serialized_version)
{
case 2:
{
mrpt::math::CPolygon stored_shape;
*f >> stored_shape;
const bool shapes_match =
( stored_shape.size()==current_robotShape.size() &&
std::equal(stored_shape.begin(),stored_shape.end(), current_robotShape.begin() ) );
if (!shapes_match) return false; // Must recompute if the robot shape changed.
}
break;
case 1:
default:
// Unknown version: Maybe we are loading a file from a more recent version of MRPT? Whatever, we can't read it: It's safer just to re-generate the PTG data
return false;
};
// Standard PTG data:
const std::string expected_desc = m_parent->getDescription();
std::string desc;
*f >> desc;
if (desc!=expected_desc) return false;
// and standard PTG data:
float ff;
uint16_t nAlphaStored;
*f >> nAlphaStored; if (nAlphaStored!=m_parent->getAlfaValuesCount()) return false;
*f >> ff; if (ff!=m_parent->getMax_V()) return false;
*f >> ff; if (ff!=m_parent->getMax_W()) return false;
// Cell dimensions:
*f >> ff; if(ff!=m_x_min) return false;
*f >> ff; if(ff!=m_x_max) return false;
*f >> ff; if(ff!=m_y_min) return false;
*f >> ff; if(ff!=m_y_max) return false;
*f >> ff; if(ff!=m_resolution) return false;
// OK, all parameters seem to be exactly the same than when we precomputed the table: load it.
//v1 was: *f >> m_map;
uint32_t N;
*f >> N;
m_map.resize(N);
for (uint32_t i=0;i<N;i++)
{
uint32_t M;
*f >> M;
m_map[i].resize(M);
for (uint32_t k=0;k<M;k++)
*f >> m_map[i][k].first >> m_map[i][k].second;
}
return true;
}
catch(std::exception &e)
{
std::cerr << "[CColisionGrid::loadFromFile] " << e.what();
return false;
}
catch(...)
{
return false;
}
}
/*---------------------------------------------------------------
build_PTG_collision_grids
---------------------------------------------------------------*/
void mrpt::reactivenav::build_PTG_collision_grids(
std::vector<CParameterizedTrajectoryGenerator*> PTGs,
const mrpt::math::CPolygon &robotShape,
const std::string &cacheFilesPrefix,
bool verbose
)
{
MRPT_START
const size_t nVerts = robotShape.verticesCount();
if (verbose)
cout << endl << "[build_PTG_collision_grids] Starting... *** THIS MAY TAKE A WHILE, BUT MUST BE COMPUTED ONLY ONCE!! **" << endl;
for (size_t nPT=0;nPT<PTGs.size();nPT++)
{
utils::CTicTac tictac;
if (verbose)
cout << "Computing collision cells for PTG[" << nPT << "]...";
ASSERT_(PTGs[nPT])
CParameterizedTrajectoryGenerator *PT = PTGs[nPT];
tictac.Tic();
// Reservar memoria para todos los puntos:
PT->allocMemForVerticesData( nVerts );
const size_t nPaths = PT->getAlfaValuesCount();
for (size_t k=0;k<nPaths;k++)
{
const size_t nPointsInPath = PT->getPointsCountInCPath_k( k );
for (size_t n=0;n<nPointsInPath;n++)
{
float x = PT->GetCPathPoint_x( k,n );
float y = PT->GetCPathPoint_y( k,n );
float phi = PT->GetCPathPoint_phi( k,n );
for (size_t m = 0;m<nVerts;m++)
{
float vx = x + cos(phi)*robotShape.GetVertex_x(m)-sin(phi)*robotShape.GetVertex_y(m);
float vy = y + sin(phi)*robotShape.GetVertex_x(m)+cos(phi)*robotShape.GetVertex_y(m);
PT->setVertex_xy(k,n,m, vx, vy );
} // for v
} // for n
} // for k
// Check for collisions between the robot shape and the grid cells:
// ----------------------------------------------------------------------------
const size_t Ki = PT->getAlfaValuesCount();
std::string auxStr = format( "%s_PTG%03u.dat.gz",cacheFilesPrefix.c_str(),static_cast<unsigned int>(nPT));
// Load the cached version, if possible
if ( PT->LoadColGridsFromFile( auxStr, robotShape ) )
{
if (verbose)
cout << "loaded from file OK" << endl;
}
else
{
// BUGFIX: In case we start reading the file and in the end detected an error,
// we must make sure that there's space enough for the grid:
PT->m_collisionGrid.setSize( -PT->refDistance,PT->refDistance,-PT->refDistance,PT->refDistance,PT->m_collisionGrid.getResolution());
// RECOMPUTE THE COLLISION GRIDS:
// ---------------------------------------
for (size_t k=0;k<Ki;k++)
{
const size_t nPoints = PT->getPointsCountInCPath_k(k);
ASSERT_(nPoints>1)
for (size_t n=0;n<(nPoints-1);n++)
{
// Check for collisions between an obstacle in a grid cell and
// the segment "s" between time steps "n" and "n+1"
for (size_t m=0;m<nVerts;m++)
{
float v1n_x, v1n_y;
float v2n_x, v2n_y;
float v1n1_x, v1n1_y;
float v2n1_x, v2n1_y;
float minx,maxx,miny,maxy;
v1n_x = PT->getVertex_x(k,n,m);
v1n_y = PT->getVertex_y(k,n,m);
v1n1_x = PT->getVertex_x(k,n+1,m);
v1n1_y = PT->getVertex_y(k,n+1,m);
v2n_x = PT->getVertex_x(k,n,(m+1) % nVerts);
v2n_y = PT->getVertex_y(k,n,(m+1) % nVerts);
v2n1_x = PT->getVertex_x(k,n+1,(m+1) % nVerts);
v2n1_y = PT->getVertex_y(k,n+1,(m+1) % nVerts);
minx=min( v1n_x, min( v2n_x, min( v1n1_x, v2n1_x ) ) );
maxx=max( v1n_x, max( v2n_x, max( v1n1_x, v2n1_x ) ) );
miny=min( v1n_y, min( v2n_y, min( v1n1_y, v2n1_y ) ) );
maxy=max( v1n_y, max( v2n_y, max( v1n1_y, v2n1_y ) ) );
// Get the range of the cell grid that is affected by the movement
// of this segment of the robot, and just check collisions at that area:
const int ix_min = PT->m_collisionGrid.x2idx(minx);
const int iy_min = PT->m_collisionGrid.y2idx(miny);
const int ix_max = PT->m_collisionGrid.x2idx(maxx);
const int iy_max = PT->m_collisionGrid.y2idx(maxy);
const float res_mid = .5f * PT->m_collisionGrid.getResolution();
for (int ix=ix_min;ix<ix_max;ix++)
{
const float cx_mid = res_mid + PT->m_collisionGrid.idx2x(ix);
for (int iy=iy_min;iy<iy_max;iy++)
{
const float cy_mid = res_mid + PT->m_collisionGrid.idx2y(iy);
if ( mrpt::math::pointIntoQuadrangle(
cx_mid,cy_mid,
v1n_x, v1n_y, v2n_x, v2n_y, v2n1_x, v2n1_y, v1n1_x, v1n1_y ) )
{
// Colision!! Update cell info:
PT->m_collisionGrid.updateCellInfo(
ix,iy,
k, // The path (Alfa value)
PT->GetCPathPoint_d(k, n+1) // The collision distance
);
}
} // for iy
} // for ix
} // for m
} // n
if (verbose)
cout << k << "/" << Ki << ",";
} // k
if (verbose)
cout << format("Done! [%.03f sec]\n",tictac.Tac() );
// save it to the cache file for the next run:
PT->SaveColGridsToFile( auxStr, robotShape );
} // "else" recompute all PTG
} // for nPT
MRPT_END
}
/*---------------------------------------------------------------
build_PTG_collision_grids for 3D reactive nav.
---------------------------------------------------------------*/
void mrpt::reactivenav::build_PTG_collision_grid3D(
CParameterizedTrajectoryGenerator* ptg,
const mrpt::math::CPolygon &robotShape,
const unsigned int level,
const unsigned int num_ptg,
bool verbose
)
{
MRPT_START
const size_t nVerts = robotShape.verticesCount();
if (verbose)
{
cout << endl << "[build_PTG_collision_grids] Starting... THIS MAY TAKE A WHILE, BUT MUST BE COMPUTED ONLY ONCE!! ";
cout << endl << "Computing collision cells for PTG[" << num_ptg << "] at level " << level << "...";
}
utils::CTicTac tictac;
tictac.Tic();
// Allocate memory for the points:
ptg->allocMemForVerticesData( nVerts );
const size_t nPaths = ptg->getAlfaValuesCount();
for (size_t k=0;k<nPaths;k++)
{
const size_t nPointsInPath = ptg->getPointsCountInCPath_k( k );
for (size_t n=0;n<nPointsInPath;n++)
{
float x = ptg->GetCPathPoint_x( k,n );
float y = ptg->GetCPathPoint_y( k,n );
float phi = ptg->GetCPathPoint_phi( k,n );
for (size_t m = 0;m<nVerts;m++)
{
float vx = x + cos(phi)*robotShape.GetVertex_x(m)-sin(phi)*robotShape.GetVertex_y(m);
float vy = y + sin(phi)*robotShape.GetVertex_x(m)+cos(phi)*robotShape.GetVertex_y(m);
ptg->setVertex_xy(k,n,m, vx, vy );
} // for v
} // for n
} // for k
// Check for collisions between the robot shape and the grid cells:
// ----------------------------------------------------------------------------
const size_t Ki = ptg->getAlfaValuesCount();
std::string auxStr = format( "CollisionGridPTG%02u_LEVEL%02u.dat.gz", num_ptg, level);
// Load the cached version, if possible
if ( ptg->LoadColGridsFromFile( auxStr, robotShape ) )
{
if (verbose)
cout << "loaded from file OK" << endl;
}
else
{
// RECOMPUTE THE COLLISION GRIDS:
// ---------------------------------------
for (size_t k=0;k<Ki;k++)
{
const size_t nPoints = ptg->getPointsCountInCPath_k(k);
ASSERT_(nPoints>1)
for (size_t n=0;n<(nPoints-1);n++)
{
// Check for collisions between an obstacle in a grid cell and
// the segment "s" between time steps "n" and "n+1"
for (size_t m=0;m<nVerts;m++)
{
float v1n_x, v1n_y;
float v2n_x, v2n_y;
float v1n1_x, v1n1_y;
float v2n1_x, v2n1_y;
float minx,maxx,miny,maxy;
v1n_x = ptg->getVertex_x(k,n,m);
v1n_y = ptg->getVertex_y(k,n,m);
v1n1_x = ptg->getVertex_x(k,n+1,m);
v1n1_y = ptg->getVertex_y(k,n+1,m);
v2n_x = ptg->getVertex_x(k,n,(m+1) % nVerts);
v2n_y = ptg->getVertex_y(k,n,(m+1) % nVerts);
v2n1_x = ptg->getVertex_x(k,n+1,(m+1) % nVerts);
v2n1_y = ptg->getVertex_y(k,n+1,(m+1) % nVerts);
minx=min( v1n_x, min( v2n_x, min( v1n1_x, v2n1_x ) ) );
maxx=max( v1n_x, max( v2n_x, max( v1n1_x, v2n1_x ) ) );
miny=min( v1n_y, min( v2n_y, min( v1n1_y, v2n1_y ) ) );
maxy=max( v1n_y, max( v2n_y, max( v1n1_y, v2n1_y ) ) );
// Get the range of the cell grid that is affected by the movement
// of this segment of the robot, and just check collisions at that area:
const int ix_min = ptg->m_collisionGrid.x2idx(minx);
const int iy_min = ptg->m_collisionGrid.y2idx(miny);
const int ix_max = ptg->m_collisionGrid.x2idx(maxx);
const int iy_max = ptg->m_collisionGrid.y2idx(maxy);
const float res_mid = .5f * ptg->m_collisionGrid.getResolution();
for (int ix=ix_min;ix<ix_max;ix++)
{
const float cx_mid = res_mid + ptg->m_collisionGrid.idx2x(ix);
for (int iy=iy_min;iy<iy_max;iy++)
{
const float cy_mid = res_mid + ptg->m_collisionGrid.idx2y(iy);
if ( mrpt::math::pointIntoQuadrangle(
cx_mid,cy_mid,
v1n_x, v1n_y, v2n_x, v2n_y, v2n1_x, v2n1_y, v1n1_x, v1n1_y ) )
{
// Colision!! Update cell info:
ptg->m_collisionGrid.updateCellInfo(
ix,iy,
k, // The path (Alfa value)
ptg->GetCPathPoint_d(k, n+1) // The collision distance
);
}
} // for iy
} // for ix
} // for m
} // n
if (verbose)
cout << k << "/" << Ki << ",";
} // k
if (verbose)
cout << format("Done! [%.03f sec]\n",tictac.Tac() );
// save it to the cache file for the next run:
ptg->SaveColGridsToFile( auxStr, robotShape );
} // "else" recompute all PTG
MRPT_END
}