void CParameterizedTrajectoryGenerator::renderPathAsSimpleLine(
	const uint16_t k, 
	mrpt::opengl::CSetOfLines &gl_obj, 
	const float decimate_distance, 
	const float max_path_distance) const
{
	const size_t nPointsInPath = getPointsCountInCPath_k(k);

	// Decimate trajectories: we don't need centimeter resolution!
	float last_added_dist = 0.0f;
	for (size_t n=0;n<nPointsInPath;n++)
	{
		const float d = GetCPathPoint_d(k, n); // distance thru path "k" until timestep "n"

		if (d<last_added_dist+decimate_distance && n!=0)
			continue; // skip: decimation

		last_added_dist = d;

		const float x = GetCPathPoint_x(k, n);
		const float y = GetCPathPoint_y(k, n);

		if (gl_obj.empty())
		     gl_obj.appendLine(0,0,0, x,y,0);
		else gl_obj.appendLineStrip(x,y,0);

		// Draw the TP only until we reach the target of the "motion" segment:
		if (max_path_distance!=0.0f && d>=max_path_distance) break;
	}
}
void CParameterizedTrajectoryGenerator::renderPathAsSimpleLine(
	const uint16_t k, 
	mrpt::opengl::CSetOfLines &gl_obj, 
	const float decimate_distance, 
	const float max_path_distance) const
{
	const size_t nPointsInPath = getPathStepCount(k);

	bool first=true;
	// Decimate trajectories: we don't need centimeter resolution!
	float last_added_dist = 0.0f;
	for (size_t n=0;n<nPointsInPath;n++)
	{
		const float d = this->getPathDist(k, n); // distance thru path "k" until timestep "n"

		if (d<last_added_dist+decimate_distance && n!=0)
			continue; // skip: decimation

		last_added_dist = d;

		mrpt::math::TPose2D p;
		this->getPathPose(k, n, p);

		if (first) {
			first=false;
			gl_obj.appendLine(0,0,0, p.x, p.y,0);
		}
		else gl_obj.appendLineStrip(p.x, p.y,0);

		// Draw the TP only until we reach the target of the "motion" segment:
		if (max_path_distance!=0.0f && d>=max_path_distance) break;
	}
}
Esempio n. 3
0
void CPTG_RobotShape_Polygonal::add_robotShape_to_setOfLines(
	mrpt::opengl::CSetOfLines& gl_shape,
	const mrpt::poses::CPose2D& origin) const
{
	const int N = m_robotShape.size();
	if (N >= 2)
	{
		// Transform coordinates:
		mrpt::math::CVectorDouble shap_x(N), shap_y(N), shap_z(N);
		for (int i = 0; i < N; i++)
		{
			origin.composePoint(
				m_robotShape[i].x, m_robotShape[i].y, 0, shap_x[i], shap_y[i],
				shap_z[i]);
		}

		gl_shape.appendLine(
			shap_x[0], shap_y[0], shap_z[0], shap_x[1], shap_y[1], shap_z[1]);
		for (int i = 0; i <= shap_x.size(); i++)
		{
			const int idx = i % shap_x.size();
			gl_shape.appendLineStrip(shap_x[idx], shap_y[idx], shap_z[idx]);
		}
	}
}
void CPTG_RobotShape_Circular::add_robotShape_to_setOfLines(
	mrpt::opengl::CSetOfLines &gl_shape,
	const mrpt::poses::CPose2D &origin) const 
{
	const double R = m_robotRadius;
	const int N = 21;
	// Transform coordinates:
	mrpt::math::CVectorDouble shap_x(N), shap_y(N),shap_z(N);
	for (int i=0;i<N;i++) {
		origin.composePoint(
			R*cos(i*2*M_PI/(N-1)),R*sin(i*2*M_PI/(N-1)), 0,
			shap_x[i],  shap_y[i],  shap_z[i]);
	}
	// Draw a "radius" to identify the "forward" orientation (phi=0)
	gl_shape.appendLine( origin.x(), origin.y(), .0, shap_x[0],shap_y[0],shap_z[0] );
	for (int i=1;i<=shap_x.size();i++) {
		const int idx = i % shap_x.size();
		gl_shape.appendLineStrip( shap_x[idx],shap_y[idx], shap_z[idx]);
	}
}
// Aux function
void add_robotShape_to_setOfLines(
	const CVectorFloat &shap_x_,
	const CVectorFloat &shap_y_, 
	mrpt::opengl::CSetOfLines &gl_shape, 
	const mrpt::poses::CPose2D &origin = mrpt::poses::CPose2D () )
{
	const int N = shap_x_.size();
	if (N>=2 && N==shap_y_.size() )
	{
		// Transform coordinates:
		CVectorDouble shap_x(N), shap_y(N),shap_z(N);
		for (int i=0;i<N;i++) {
			origin.composePoint(
				shap_x_[i], shap_y_[i], 0,
				shap_x[i],  shap_y[i],  shap_z[i]);
		}

		gl_shape.appendLine( shap_x[0], shap_y[0], shap_z[0], shap_x[1],shap_y[1],shap_z[1] );
		for (int i=0;i<=shap_x.size();i++) {
			const int idx = i % shap_x.size();
			gl_shape.appendLineStrip( shap_x[idx],shap_y[idx], shap_z[idx]);
		}
	}
}