CPTG_DiffDrive_C.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, 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 "nav-precomp.h" // Precomp header
#include <mrpt/nav/tpspace/CPTG_DiffDrive_C.h>
#include <mrpt/math/wrap2pi.h>

using namespace mrpt;
using namespace mrpt::nav;
using namespace mrpt::utils;
using namespace mrpt::system;

IMPLEMENTS_SERIALIZABLE(CPTG_DiffDrive_C,CParameterizedTrajectoryGenerator,mrpt::nav)


void CPTG_DiffDrive_C::loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg,const std::string &sSection)
{
        CPTG_DiffDrive_CollisionGridBased ::loadFromConfigFile(cfg,sSection);
        
        MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(K,double, cfg,sSection);
}
void CPTG_DiffDrive_C::saveToConfigFile(mrpt::utils::CConfigFileBase &cfg,const std::string &sSection) const
{
        MRPT_START
        const int WN = 25, WV = 30;
        CPTG_DiffDrive_CollisionGridBased::saveToConfigFile(cfg,sSection);

        cfg.write(sSection,"K",K,   WN,WV, "K=+1 forward paths; K=-1 for backwards paths.");

        MRPT_END
}

void CPTG_DiffDrive_C::readFromStream(mrpt::utils::CStream &in, int version)
{
        CPTG_DiffDrive_CollisionGridBased::internal_readFromStream(in);

        switch (version)
        {
        case 0:
                in >> K; 
                break;
        default:
                MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}

void CPTG_DiffDrive_C::writeToStream(mrpt::utils::CStream &out, int *version) const
{
        if (version) 
        {
                *version = 0;
                return;
        }

        CPTG_DiffDrive_CollisionGridBased::internal_writeToStream(out);
        out << K;
}



std::string CPTG_DiffDrive_C::getDescription() const
{
        return mrpt::format("CPTG_DiffDrive_C,K=%i",(int)K);
}

void CPTG_DiffDrive_C::ptgDiffDriveSteeringFunction( float alpha, float t,float x, float y, float phi, float &v, float &w ) const
{
        MRPT_UNUSED_PARAM(t); MRPT_UNUSED_PARAM(x); MRPT_UNUSED_PARAM(y); MRPT_UNUSED_PARAM(phi);
    // (v,w)
    v = V_MAX * sign(K);
        // Use a linear mapping:  (Old was: w = tan( alpha/2 ) * W_MAX * sign(K))
    w = (alpha/M_PI) * W_MAX * sign(K); 
}

bool CPTG_DiffDrive_C::PTG_IsIntoDomain( double x, double y ) const
{
        MRPT_UNUSED_PARAM(x); MRPT_UNUSED_PARAM(y);
        return true;
}

bool CPTG_DiffDrive_C::inverseMap_WS2TP(double x, double y, int &k_out, double &d_out, double tolerance_dist) const
{
        MRPT_UNUSED_PARAM(tolerance_dist);
        bool is_exact = true;
        if (y!=0)
        {
                double R = (x*x+y*y)/(2*y);
                const double Rmin = std::abs(V_MAX/W_MAX);

                double theta;

                if (K>0)
                {
                        if (y>0)
                                        theta = atan2( (double)x,fabs(R)-y );
                        else    theta = atan2( (double)x,y+fabs(R) );
                }
                else
                {
                        if (y>0)
                                        theta = atan2( -(double)x,fabs(R)-y );
                        else    theta = atan2( -(double)x,y+fabs(R) );
                }

                // Arc length must be possitive [0,2*pi]
                mrpt::math::wrapTo2PiInPlace(theta);

                // Distance thru arc:
                d_out = (float)(theta * (fabs(R)+turningRadiusReference));

                if (std::abs(R)<Rmin)
                {
                        is_exact=false;
                        R=Rmin*mrpt::utils::sign(R);
                }
                
                //Was: a = 2*atan( V_MAX / (W_MAX*R) );
                const double a = M_PI* V_MAX / (W_MAX*R);
                k_out = alpha2index( (float)a );

        }
        else
        {
                if (sign(x)==sign(K))
                {
                        k_out = alpha2index(0);
                        d_out = x;
                        is_exact=true;
                }
                else
                {
                        k_out = m_alphaValuesCount-1;
                        d_out = 1e+3;
                        is_exact=false;
                }
        }

        // Normalize:
        d_out = d_out / refDistance;

        ASSERT_ABOVEEQ_(k_out,0)
        ASSERT_BELOW_(k_out,m_alphaValuesCount)

        return is_exact;
}

void CPTG_DiffDrive_C::loadDefaultParams()
{
        CPTG_DiffDrive_CollisionGridBased::loadDefaultParams();
        K = +1.0;
}