build_PTG_collision_grids(): simplified and more accurate

Leave only one build_PTG_collision_grids() function.
Slighly slower but more accurate and conservative (safer) computation of
collision grids for PTGs.

Author: jlblancoc

doc/doxygen-pages/changeLog_doc.h

@@ -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.

libs/reactivenav/include/mrpt/reactivenav/CParameterizedTrajectoryGenerator.h

@@ -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; }
@@ -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();
 

libs/reactivenav/include/mrpt/reactivenav/motion_planning_utils.h

@@ -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
 			);
 
 	/** @} */

libs/reactivenav/src/CParameterizedTrajectoryGenerator.cpp

@@ -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"] );
@@ -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.

libs/reactivenav/src/CReactiveNavigationSystem.cpp

@@ -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");
 	}

libs/reactivenav/src/CReactiveNavigationSystem3D.cpp

@@ -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*/ 
+					);
 			}
 		}