CVehicleVelCmd_DiffDriven.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 "kinematics-precomp.h"  // Precompiled header
#include <mrpt/kinematics/CVehicleVelCmd_DiffDriven.h>
#include <mrpt/utils/CStream.h>

using namespace mrpt::kinematics;
using namespace mrpt::utils;

IMPLEMENTS_SERIALIZABLE(
        CVehicleVelCmd_DiffDriven, CVehicleVelCmd, mrpt::kinematics)

CVehicleVelCmd_DiffDriven::CVehicleVelCmd_DiffDriven()
        : lin_vel(.0), ang_vel(.0)
{
}
CVehicleVelCmd_DiffDriven::~CVehicleVelCmd_DiffDriven() {}
size_t CVehicleVelCmd_DiffDriven::getVelCmdLength() const { return 2; }
std::string CVehicleVelCmd_DiffDriven::getVelCmdDescription(
        const int index) const
{
        switch (index)
        {
                case 0:
                        return "lin_vel";
                        break;
                case 1:
                        return "ang_vel";
                        break;
                default:
                        THROW_EXCEPTION_FMT("index out of bounds: %i", index);
        };
}

double CVehicleVelCmd_DiffDriven::getVelCmdElement(const int index) const
{
        switch (index)
        {
                case 0:
                        return lin_vel;
                        break;
                case 1:
                        return ang_vel;
                        break;
                default:
                        THROW_EXCEPTION_FMT("index out of bounds: %i", index);
        };
}

void CVehicleVelCmd_DiffDriven::setVelCmdElement(
        const int index, const double val)
{
        switch (index)
        {
                case 0:
                        lin_vel = val;
                        break;
                case 1:
                        ang_vel = val;
                        break;
                default:
                        THROW_EXCEPTION_FMT("index out of bounds: %i", index);
        };
}

bool CVehicleVelCmd_DiffDriven::isStopCmd() const
{
        return lin_vel == .0 && ang_vel == .0;
}

void CVehicleVelCmd_DiffDriven::setToStop() { lin_vel = ang_vel = .0; }
void CVehicleVelCmd_DiffDriven::readFromStream(
        mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                        in >> lin_vel >> ang_vel;
                        break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}

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

void CVehicleVelCmd_DiffDriven::cmdVel_scale(double vel_scale)
{
        lin_vel *= vel_scale;
        ang_vel *= vel_scale;
}

double CVehicleVelCmd_DiffDriven::cmdVel_limits(
        const mrpt::kinematics::CVehicleVelCmd& prev_vel_cmd, const double beta,
        const TVelCmdParams& params)
{
        ASSERT_(params.robotMax_V_mps > 0);
        ASSERT_(params.robotMax_W_radps > 0);
        const mrpt::kinematics::CVehicleVelCmd_DiffDriven* prevcmd =
                dynamic_cast<const mrpt::kinematics::CVehicleVelCmd_DiffDriven*>(
                        &prev_vel_cmd);
        ASSERTMSG_(prevcmd, "Expected prevcmd of type `CVehicleVelCmd_DiffDriven`");

        double speed_scale = filter_max_vw(lin_vel, ang_vel, params);

        if (std::abs(lin_vel) <
                0.01)  // i.e. new behavior is nearly a pure rotation
        {  // thus, it's OK to blend the rotational component
                ang_vel = beta * ang_vel + (1 - beta) * prevcmd->ang_vel;
        }
        else  // there is a non-zero translational component
        {
                // must maintain the ratio of w to v (while filtering v)
                float ratio = ang_vel / lin_vel;
                lin_vel = beta * lin_vel +
                                  (1 - beta) * prevcmd->lin_vel;  // blend new v value
                ang_vel =
                        ratio * lin_vel;  // ensure new w implements expected path curvature

                speed_scale *= filter_max_vw(lin_vel, ang_vel, params);
        }

        return speed_scale;
}

double CVehicleVelCmd_DiffDriven::filter_max_vw(
        double& v, double& w, const TVelCmdParams& p)
{
        double speed_scale = 1.0;
        // Ensure maximum speeds:
        if (std::abs(v) > p.robotMax_V_mps)
        {
                // Scale:
                const double F = std::abs(p.robotMax_V_mps / v);
                v *= F;
                w *= F;
                speed_scale *= F;
        }

        if (std::abs(w) > p.robotMax_W_radps)
        {
                // Scale:
                const double F = std::abs(p.robotMax_W_radps / w);
                v *= F;
                w *= F;
                speed_scale *= F;
        }
        return speed_scale;
}