<commit_before>/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2014, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */


#include <mrpt/reactivenav.h>  // Precomp header

#include <mrpt/utils/CStartUpClassesRegister.h>
extern mrpt::utils::CStartUpClassesRegister  mrpt_reactivenav_class_reg;
const int dumm = mrpt_reactivenav_class_reg.do_nothing(); // Avoid compiler removing this class in static linking


#include <mrpt/utils/CFileGZInputStream.h>
#include <mrpt/utils/CFileGZOutputStream.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/math/geometry.h>

using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::reactivenav;
using namespace mrpt::system;
using namespace std;

/** Constructor: possible values in "params":
 *   - ref_distance: The maximum distance in PTGs
 *   - resolution: The cell size
 *   - v_max, w_max: Maximum robot speeds.
 */
CParameterizedTrajectoryGenerator::CParameterizedTrajectoryGenerator(const TParameters<double> &params) :
	m_collisionGrid(-1,1,-1,1,0.5,this)
{
	this->refDistance	= params["ref_distance"];
	this->V_MAX			= params["v_max"];
	this->W_MAX			= params["w_max"];

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

	initializeCollisionsGrid( refDistance, params["resolution"] );
}

/*---------------------------------------------------------------
					Class factory
  ---------------------------------------------------------------*/
CParameterizedTrajectoryGenerator * CParameterizedTrajectoryGenerator::CreatePTG(const TParameters<double> &params)
{
	MRPT_START
	const int nPTG = static_cast<int>( params["PTG_type"] );
	switch(nPTG)
	{
	case 1: return new CPTG1(params);
	case 2: return new CPTG2(params);
	case 3: return new CPTG3(params);
	case 4: return new CPTG4(params);
	case 5: return new CPTG5(params);
	case 6: return new CPTG6(params);
	case 7: return new CPTG7(params);
	default:
		THROW_EXCEPTION_CUSTOM_MSG1("Unknown PTG_type=%i",nPTG)
	};
	MRPT_END
}

/*---------------------------------------------------------------
					initializeCollisionsGrid
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::initializeCollisionsGrid(float refDistance,float resolution)
{
	m_collisionGrid.setSize( -refDistance,refDistance,-refDistance,refDistance,resolution );
}

/*---------------------------------------------------------------
					FreeMemory
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::FreeMemory()
{
	if (m_alphaValuesCount)
	{
		// 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.
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::simulateTrajectories(
		uint16_t	    alphaValuesCount,
		float			max_time,
		float			max_dist,
		unsigned int	max_n,
		float			diferencial_t,
		float			min_dist,
		float			*out_max_acc_v,
		float			*out_max_acc_w)
{
		// Primero, liberar memoria:
		FreeMemory();

		// The number of discreet values for ALPHA:
		this->m_alphaValuesCount = alphaValuesCount;

		// Reserve the size in the buffers:
		CPoints.resize( m_alphaValuesCount );

		// Calcular maxima distancia del contorno del robot (para 1 calculo auxiliar)
		float  radio_max_robot=1.0;    // Aprox.

		// Aux buffer:
		TCPointVector	points;

		float          ult_dist, ult_dist1, ult_dist2;

		// For the grid:
		float		   x_min = 1e3f, x_max = -1e3;
		float		   y_min = 1e3f, y_max = -1e3;

		// Para averiguar las maximas ACELERACIONES lineales y angulares:
		float			max_acc_lin, max_acc_ang;

		maxV_inTPSpace = 0;
		max_acc_lin = max_acc_ang = 0;

	try
	{
		for (unsigned int k=0;k<m_alphaValuesCount;k++)
		{
			// Simulate / evaluate the trajectory selected by this "alpha":
			// ------------------------------------------------------------
			const float alpha = index2alpha( k );

			points.clear();
			float t = .0f, dist = .0f, girado = .0f;
			float x = .0f, y = .0f, phi = .0f, v = .0f, w = .0f, _x = .0f, _y = .0f, _phi = .0f;

			// Sliding window with latest movement commands (for the optional low-pass filtering):
			float  last_vs[2] = {.0f,.0f}, last_ws[2] = {.0f,.0f};

			// Add the first, initial point:
			points.push_back( TCPoint(	x,y,phi, t,dist, v,w ) );

			// Simulate until...
			while ( t < max_time && dist < max_dist && points.size() < max_n && fabs(girado) < 1.95 * M_PI )
			{
				// Max. aceleraciones:
				if (t>1)
				{
					float acc_lin = fabs( (last_vs[0]-last_vs[1])/diferencial_t);
					float acc_ang = fabs( (last_ws[0]-last_ws[1])/diferencial_t);
					mrpt::utils::keep_max(max_acc_lin, acc_lin);
					mrpt::utils::keep_max(max_acc_ang, acc_ang);
				}

				// Compute new movement command (v,w):
				PTG_Generator( alpha,t, x, y, phi, v,w );

				if (t==0)
					mrpt::utils::keep_max(maxV_inTPSpace, (float)( sqrt( square(v) + square(w*turningRadiusReference) ) ) );

				// History of v/w ----------------------------------
				last_vs[1]=last_vs[0];
				last_ws[1]=last_ws[0];
				last_vs[0] = v;
				last_ws[0] = w;
				// -------------------------------------------

				// Finite difference equation:
				x += cos(phi)* v * diferencial_t;
				y += sin(phi)* v * diferencial_t;
				phi+= w * diferencial_t;

				// Counters:
				girado += w * diferencial_t;

				float v_inTPSpace = sqrt( square(v)+square(w*turningRadiusReference) );

				dist += v_inTPSpace  * diferencial_t;

				t += diferencial_t;

				// Save sample if we moved far enough:
				ult_dist1 = sqrt( square( _x - x )+square( _y - y  ) );
				ult_dist2 = fabs( radio_max_robot* ( _phi - phi ) );
				ult_dist = max( ult_dist1, ult_dist2 );

				if (ult_dist > min_dist)
				{
					// Set the (v,w) to the last record:
					points.back().v = v;
					points.back().w = w;

					// And add the new record:
					points.push_back( TCPoint(	x,y,phi,t,dist,v,w) );

					// For the next iter:
					_x = x;
					_y = y;
					_phi = phi;
				}

				// for the grid:
				x_min = min(x_min,x); x_max = max(x_max,x);
				y_min = min(y_min,y); y_max = max(y_max,y);
			}

			// Add the final point:
			points.back().v = v;
			points.back().w = w;
			points.push_back( TCPoint(	x,y,phi,t,dist,v,w) );

			// Save data to C-Space path structure:
			CPoints[k] = points;

		} // end for "k"

		// Save accelerations
		if (out_max_acc_v) *out_max_acc_v = max_acc_lin;
		if (out_max_acc_w) *out_max_acc_w = max_acc_ang;

		// --------------------------------------------------------
		// Build the speeding-up grid for lambda function:
		// --------------------------------------------------------
		const TCellForLambdaFunction defaultCell;
		m_lambdaFunctionOptimizer.setSize(
			x_min-0.5f,x_max+0.5f,
			y_min-0.5f,y_max+0.5f,  0.25f,
			&defaultCell);

		for (uint16_t k=0;k<m_alphaValuesCount;k++)
		{
			const uint32_t M = static_cast<uint32_t>(CPoints[k].size());
			for (uint32_t n=0;n<M;n++)
			{
				TCellForLambdaFunction	*cell = m_lambdaFunctionOptimizer.cellByPos(CPoints[k][n].x,CPoints[k][n].y);
				ASSERT_(cell)
				// Keep limits:
				mrpt::utils::keep_min(cell->k_min, k );
				mrpt::utils::keep_max(cell->k_max, k );
				mrpt::utils::keep_min(cell->n_min, n );
				mrpt::utils::keep_max(cell->n_max, n );
			}
		}
	}
	catch(...)
	{
		std::cout << format("[CParameterizedTrajectoryGenerator::simulateTrajectories] Simulation aborted: unexpected exception!\n");
	}

}

/*---------------------------------------------------------------
					directionToMotionCommand
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::directionToMotionCommand( uint16_t k, float &v, float &w )
{
	PTG_Generator( index2alpha(k),0, 0, 0, 0, v, w );
}

/*---------------------------------------------------------------
					getCPointWhen_d_Is
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::getCPointWhen_d_Is (
		float		d,
		uint16_t k,
		float		&x,
		float		&y,
		float		&phi,
		float		&t,
				float *v,
				float *w)
{
		unsigned int     n=0;

		if (k>=m_alphaValuesCount)
		{
			x=y=phi=0;
			return;  // Por si acaso
		}

		while ( n < (CPoints[k].size()-1) && CPoints[k][n].dist<d )
				n++;

		x=CPoints[k][n].x;
		y=CPoints[k][n].y;
		phi=CPoints[k][n].phi;
		t=CPoints[k][n].t;
		if (v) *v =CPoints[k][n].v;
		if (w) *w =CPoints[k][n].w;
}

/*---------------------------------------------------------------
						debugDumpInFiles
  ---------------------------------------------------------------*/
bool CParameterizedTrajectoryGenerator::debugDumpInFiles( const int nPT )
{
	mrpt::system::createDirectory( "./reactivenav.logs" );<commit_msg>PTG: getCPointWhen_d_Is() safer (conservative) points returned now<commit_after>/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2014, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */


#include <mrpt/reactivenav.h>  // Precomp header

#include <mrpt/utils/CStartUpClassesRegister.h>
extern mrpt::utils::CStartUpClassesRegister  mrpt_reactivenav_class_reg;
const int dumm = mrpt_reactivenav_class_reg.do_nothing(); // Avoid compiler removing this class in static linking


#include <mrpt/utils/CFileGZInputStream.h>
#include <mrpt/utils/CFileGZOutputStream.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/math/geometry.h>

using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::reactivenav;
using namespace mrpt::system;
using namespace std;

/** Constructor: possible values in "params":
 *   - ref_distance: The maximum distance in PTGs
 *   - resolution: The cell size
 *   - v_max, w_max: Maximum robot speeds.
 */
CParameterizedTrajectoryGenerator::CParameterizedTrajectoryGenerator(const TParameters<double> &params) :
	m_collisionGrid(-1,1,-1,1,0.5,this)
{
	this->refDistance	= params["ref_distance"];
	this->V_MAX			= params["v_max"];
	this->W_MAX			= params["w_max"];

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

	initializeCollisionsGrid( refDistance, params["resolution"] );
}

/*---------------------------------------------------------------
					Class factory
  ---------------------------------------------------------------*/
CParameterizedTrajectoryGenerator * CParameterizedTrajectoryGenerator::CreatePTG(const TParameters<double> &params)
{
	MRPT_START
	const int nPTG = static_cast<int>( params["PTG_type"] );
	switch(nPTG)
	{
	case 1: return new CPTG1(params);
	case 2: return new CPTG2(params);
	case 3: return new CPTG3(params);
	case 4: return new CPTG4(params);
	case 5: return new CPTG5(params);
	case 6: return new CPTG6(params);
	case 7: return new CPTG7(params);
	default:
		THROW_EXCEPTION_CUSTOM_MSG1("Unknown PTG_type=%i",nPTG)
	};
	MRPT_END
}

/*---------------------------------------------------------------
					initializeCollisionsGrid
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::initializeCollisionsGrid(float refDistance,float resolution)
{
	m_collisionGrid.setSize( -refDistance,refDistance,-refDistance,refDistance,resolution );
}

/*---------------------------------------------------------------
					FreeMemory
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::FreeMemory()
{
	if (m_alphaValuesCount)
	{
		// 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.
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::simulateTrajectories(
		uint16_t	    alphaValuesCount,
		float			max_time,
		float			max_dist,
		unsigned int	max_n,
		float			diferencial_t,
		float			min_dist,
		float			*out_max_acc_v,
		float			*out_max_acc_w)
{
		// Primero, liberar memoria:
		FreeMemory();

		// The number of discreet values for ALPHA:
		this->m_alphaValuesCount = alphaValuesCount;

		// Reserve the size in the buffers:
		CPoints.resize( m_alphaValuesCount );

		// Calcular maxima distancia del contorno del robot (para 1 calculo auxiliar)
		float  radio_max_robot=1.0;    // Aprox.

		// Aux buffer:
		TCPointVector	points;

		float          ult_dist, ult_dist1, ult_dist2;

		// For the grid:
		float		   x_min = 1e3f, x_max = -1e3;
		float		   y_min = 1e3f, y_max = -1e3;

		// Para averiguar las maximas ACELERACIONES lineales y angulares:
		float			max_acc_lin, max_acc_ang;

		maxV_inTPSpace = 0;
		max_acc_lin = max_acc_ang = 0;

	try
	{
		for (unsigned int k=0;k<m_alphaValuesCount;k++)
		{
			// Simulate / evaluate the trajectory selected by this "alpha":
			// ------------------------------------------------------------
			const float alpha = index2alpha( k );

			points.clear();
			float t = .0f, dist = .0f, girado = .0f;
			float x = .0f, y = .0f, phi = .0f, v = .0f, w = .0f, _x = .0f, _y = .0f, _phi = .0f;

			// Sliding window with latest movement commands (for the optional low-pass filtering):
			float  last_vs[2] = {.0f,.0f}, last_ws[2] = {.0f,.0f};

			// Add the first, initial point:
			points.push_back( TCPoint(	x,y,phi, t,dist, v,w ) );

			// Simulate until...
			while ( t < max_time && dist < max_dist && points.size() < max_n && fabs(girado) < 1.95 * M_PI )
			{
				// Max. aceleraciones:
				if (t>1)
				{
					float acc_lin = fabs( (last_vs[0]-last_vs[1])/diferencial_t);
					float acc_ang = fabs( (last_ws[0]-last_ws[1])/diferencial_t);
					mrpt::utils::keep_max(max_acc_lin, acc_lin);
					mrpt::utils::keep_max(max_acc_ang, acc_ang);
				}

				// Compute new movement command (v,w):
				PTG_Generator( alpha,t, x, y, phi, v,w );

				if (t==0)
					mrpt::utils::keep_max(maxV_inTPSpace, (float)( sqrt( square(v) + square(w*turningRadiusReference) ) ) );

				// History of v/w ----------------------------------
				last_vs[1]=last_vs[0];
				last_ws[1]=last_ws[0];
				last_vs[0] = v;
				last_ws[0] = w;
				// -------------------------------------------

				// Finite difference equation:
				x += cos(phi)* v * diferencial_t;
				y += sin(phi)* v * diferencial_t;
				phi+= w * diferencial_t;

				// Counters:
				girado += w * diferencial_t;

				float v_inTPSpace = sqrt( square(v)+square(w*turningRadiusReference) );

				dist += v_inTPSpace  * diferencial_t;

				t += diferencial_t;

				// Save sample if we moved far enough:
				ult_dist1 = sqrt( square( _x - x )+square( _y - y  ) );
				ult_dist2 = fabs( radio_max_robot* ( _phi - phi ) );
				ult_dist = max( ult_dist1, ult_dist2 );

				if (ult_dist > min_dist)
				{
					// Set the (v,w) to the last record:
					points.back().v = v;
					points.back().w = w;

					// And add the new record:
					points.push_back( TCPoint(	x,y,phi,t,dist,v,w) );

					// For the next iter:
					_x = x;
					_y = y;
					_phi = phi;
				}

				// for the grid:
				x_min = min(x_min,x); x_max = max(x_max,x);
				y_min = min(y_min,y); y_max = max(y_max,y);
			}

			// Add the final point:
			points.back().v = v;
			points.back().w = w;
			points.push_back( TCPoint(	x,y,phi,t,dist,v,w) );

			// Save data to C-Space path structure:
			CPoints[k] = points;

		} // end for "k"

		// Save accelerations
		if (out_max_acc_v) *out_max_acc_v = max_acc_lin;
		if (out_max_acc_w) *out_max_acc_w = max_acc_ang;

		// --------------------------------------------------------
		// Build the speeding-up grid for lambda function:
		// --------------------------------------------------------
		const TCellForLambdaFunction defaultCell;
		m_lambdaFunctionOptimizer.setSize(
			x_min-0.5f,x_max+0.5f,
			y_min-0.5f,y_max+0.5f,  0.25f,
			&defaultCell);

		for (uint16_t k=0;k<m_alphaValuesCount;k++)
		{
			const uint32_t M = static_cast<uint32_t>(CPoints[k].size());
			for (uint32_t n=0;n<M;n++)
			{
				TCellForLambdaFunction	*cell = m_lambdaFunctionOptimizer.cellByPos(CPoints[k][n].x,CPoints[k][n].y);
				ASSERT_(cell)
				// Keep limits:
				mrpt::utils::keep_min(cell->k_min, k );
				mrpt::utils::keep_max(cell->k_max, k );
				mrpt::utils::keep_min(cell->n_min, n );
				mrpt::utils::keep_max(cell->n_max, n );
			}
		}
	}
	catch(...)
	{
		std::cout << format("[CParameterizedTrajectoryGenerator::simulateTrajectories] Simulation aborted: unexpected exception!\n");
	}

}

/*---------------------------------------------------------------
					directionToMotionCommand
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::directionToMotionCommand( uint16_t k, float &v, float &w )
{
	PTG_Generator( index2alpha(k),0, 0, 0, 0, v, w );
}

/*---------------------------------------------------------------
					getCPointWhen_d_Is
  ---------------------------------------------------------------*/
void CParameterizedTrajectoryGenerator::getCPointWhen_d_Is (
		float		d,
		uint16_t k,
		float		&x,
		float		&y,
		float		&phi,
		float		&t,
				float *v,
				float *w)
{
		if (k>=m_alphaValuesCount)
		{
			x=y=phi=0;
			return;  // Por si acaso
		}

		unsigned int n=0;
		const unsigned int numPoints = CPoints[k].size();
		for ( ; (n+1) < numPoints ; n++)
		{
			if (CPoints[k][n+1].dist>=d)
				break;
		}

		x=CPoints[k][n].x;
		y=CPoints[k][n].y;
		phi=CPoints[k][n].phi;
		t=CPoints[k][n].t;
		if (v) *v =CPoints[k][n].v;
		if (w) *w =CPoints[k][n].w;
}

/*---------------------------------------------------------------
						debugDumpInFiles
  ---------------------------------------------------------------*/
bool CParameterizedTrajectoryGenerator::debugDumpInFiles( const int nPT )
{
	mrpt::system::createDirectory( "./reactivenav.logs" );
{'fraction_non_alphanumeric': 0.2553933854243452, 'fraction_numerical': 0.012531328320802004, 'mean_word_length': 1.903824200913242, 'pattern_counts': {'":': 4, '<': 46, '<?xml version=': 0, '>': 48, 'https://': 0, 'lorem ipsum': 0, 'www.': 6, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6958232', 'n_tokens_mistral': 7663, 'n_tokens_neox': 6717, 'n_words': 1395}