Skip to content

Commit

Permalink
build_PTG_collision_grids(): simplified and more accurate
Browse files Browse the repository at this point in the history
Leave only one build_PTG_collision_grids() function.
Slighly slower but more accurate and conservative (safer) computation of
collision grids for PTGs.
  • Loading branch information
jlblancoc committed Feb 3, 2014
1 parent 0a12607 commit 7bd68e4
Show file tree
Hide file tree
Showing 7 changed files with 89 additions and 345 deletions.
1 change: 1 addition & 0 deletions doc/doxygen-pages/changeLog_doc.h
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@
- mrpt::reactivenav::CParameterizedTrajectoryGenerator: New methods:
- mrpt::reactivenav::CParameterizedTrajectoryGenerator::inverseMap_WS2TP() for inverse look-up of WS to TP space - [(commit)](https://github.com/jlblancoc/mrpt/commit/4d04ef50e3dea581bed6287d4ea6593034c47da3)
- mrpt::reactivenav::CParameterizedTrajectoryGenerator::renderPathAsSimpleLine() - [(commit)](https://github.com/jlblancoc/mrpt/commit/a224fc2489ad00b3ab116c84e8d4a48532a005df)
- Changed the signature of mrpt::reactivenav::build_PTG_collision_grids() to become more generic for 2D & 2.5D PTGs - [(commit)]()
- Deleted classes:
- mrpt::utils::CEvent, which was actually unimplemented (!)
- mrpt::hwdrivers::CInterfaceNI845x has been deleted. It didn't offer features enough to justify a class.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -116,42 +116,6 @@ namespace mrpt
float GetCPathPoint_v( uint16_t k, int n ) const { return CPoints[k][n].v; }
float GetCPathPoint_w( uint16_t k, int n ) const { return CPoints[k][n].w; }

void allocMemForVerticesData( int nVertices );

void setVertex_xy( uint16_t k, int n, int m, float x, float y )
{
vertexPoints_x[k][ n*nVertices + m ] = x;
vertexPoints_y[k][ n*nVertices + m ] = y;
}

float getVertex_x( uint16_t k, int n, int m ) const
{
int idx = n*nVertices + m;
// assert( idx>=0);assert(idx<nVertices * nPointsInEachPath[k] );
return vertexPoints_x[k][idx];
}

float getVertex_y( uint16_t k, int n, int m ) const
{
int idx = n*nVertices + m;
// assert( idx>=0);assert(idx<nVertices * nPointsInEachPath[k] );
return vertexPoints_y[k][idx];
}

float* getVertixesArray_x( uint16_t k, int n )
{
int idx = n*nVertices;
return &vertexPoints_x[k][idx];
}

float* getVertixesArray_y( uint16_t k, int n )
{
int idx = n*nVertices;
return &vertexPoints_y[k][idx];
}

unsigned int getVertixesCount() const { return nVertices; }

float getMax_V() const { return V_MAX; }
float getMax_W() const { return W_MAX; }
float getMax_V_inTPSpace() const { return maxV_inTPSpace; }
Expand Down Expand Up @@ -301,11 +265,6 @@ namespace mrpt
typedef std::vector<TCPoint> TCPointVector;
std::vector<TCPointVector> CPoints;

/** The shape of the robot along the trajectories:
*/
std::vector<vector_float> vertexPoints_x,vertexPoints_y;
int nVertices;

/** Free all the memory buffers */
void FreeMemory();

Expand Down
22 changes: 6 additions & 16 deletions libs/reactivenav/include/mrpt/reactivenav/motion_planning_utils.h
Original file line number Diff line number Diff line change
Expand Up @@ -20,29 +20,19 @@ namespace mrpt
/** @name Motion planning utilities
@{*/

/** Builds the collision grid for a given list of PTGs.
/** Builds the collision grid for a given PTGs, or load it from a cache file.
* The collision grid must be calculated before calling CParameterizedTrajectoryGenerator::CColisionGrid::getTPObstacle
* \param PTGs The list of PTGs to calculate their grids.
* \param robotShape The shape of the robot.
* \param cacheFilesPrefix The prefix of the files where the collision grids will be dumped to speed-up future recalculations.
* \param verbose
* \param cacheFilename The filename where the collision grids will be dumped to speed-up future recalculations. If it exists upon call, the collision grid will be loaded from here if all PTG parameters match. Example: "PTG_%03d.dat.gz".
* \sa CReactiveNavigationSystem
* \ingroup mrpt_reactivenav_grp
*/

void REACTIVENAV_IMPEXP build_PTG_collision_grids(
std::vector<CParameterizedTrajectoryGenerator*> PTGs,
const mrpt::math::CPolygon &robotShape,
const std::string &cacheFilesPrefix = std::string("ReacNavGrid_"),
bool verbose = true
);

void REACTIVENAV_IMPEXP build_PTG_collision_grid3D(
CParameterizedTrajectoryGenerator* ptg,
const mrpt::math::CPolygon &robotShape,
const unsigned int level,
const unsigned int num_ptg,
bool verbose = true
CParameterizedTrajectoryGenerator * PTG,
const mrpt::math::CPolygon & robotShape,
const std::string & cacheFilename,
const bool verbose = true
);

/** @} */
Expand Down
24 changes: 0 additions & 24 deletions libs/reactivenav/src/CParameterizedTrajectoryGenerator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,6 @@ CParameterizedTrajectoryGenerator::CParameterizedTrajectoryGenerator(const TPara
this->W_MAX = params["w_max"];

m_alphaValuesCount=0;
nVertices = 0;
turningRadiusReference = 0.10f;

initializeCollisionsGrid( refDistance, params["resolution"] );
Expand Down Expand Up @@ -85,34 +84,11 @@ void CParameterizedTrajectoryGenerator::FreeMemory()
// Free trajectories:
CPoints.clear();

// And the shape of the robot along them:
vertexPoints_x.clear();
vertexPoints_y.clear();

// Signal an empty PTG:
m_alphaValuesCount = 0;
}
}

/*---------------------------------------------------------------
allocMemFoVerticesData
---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::allocMemForVerticesData( int nVertices )
{
vertexPoints_x.resize(m_alphaValuesCount);
vertexPoints_y.resize(m_alphaValuesCount);

// Alloc the exact number of items, all of them set to 0:
for (unsigned int i=0;i<m_alphaValuesCount;i++)
{
vertexPoints_x[i].resize( nVertices * getPointsCountInCPath_k(i), 0 );
vertexPoints_y[i].resize( nVertices * getPointsCountInCPath_k(i), 0 );
}

// Save it:
this->nVertices= nVertices;
}

/*---------------------------------------------------------------
simulateTrajectories
Solve trajectories and fill cells.
Expand Down
14 changes: 9 additions & 5 deletions libs/reactivenav/src/CReactiveNavigationSystem.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -202,11 +202,15 @@ void CReactiveNavigationSystem::STEP1_CollisionGridsBuilder()

m_timelogger.enter("build_PTG_collision_grids");

mrpt::reactivenav::build_PTG_collision_grids(
PTGs,
m_robotShape,
format("ReacNavGrid_%s",robotName.c_str())
);
for (unsigned int i=0;i<PTGs.size();i++)
{
mrpt::reactivenav::build_PTG_collision_grids(
PTGs[i],
m_robotShape,
format("ReacNavGrid_%s_%03u.dat.gz",robotName.c_str(),i),
m_enableConsoleOutput /*verbose*/
);
}

m_timelogger.leave("build_PTG_collision_grids");
}
Expand Down
7 changes: 6 additions & 1 deletion libs/reactivenav/src/CReactiveNavigationSystem3D.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -215,7 +215,12 @@ void CReactiveNavigationSystem3D::STEP1_CollisionGridsBuilder()
{
for (unsigned int i=0; i<m_robotShape.heights.size(); i++)
{
build_PTG_collision_grid3D(m_ptgmultilevel[j].PTGs[i], m_robotShape.polygons[i], i+1, j+1, m_enableConsoleOutput /*VERBOSE*/ );
mrpt::reactivenav::build_PTG_collision_grids(
m_ptgmultilevel[j].PTGs[i],
m_robotShape.polygons[i],
format("ReacNavGrid_%s_%03u_L%02u.dat.gz",robotName.c_str(),i,j),
m_enableConsoleOutput /*VERBOSE*/
);
}
}

Expand Down

0 comments on commit 7bd68e4

Please sign in to comment.