CHolonomicVFF.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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/holonomic/CHolonomicVFF.h>
#include <mrpt/serialization/CArchive.h>
 
using namespace mrpt;
using namespace std;
 
using namespace mrpt::nav;
 
IMPLEMENTS_SERIALIZABLE(CLogFileRecord_VFF, CHolonomicLogFileRecord, mrpt::nav)
IMPLEMENTS_SERIALIZABLE(
        CHolonomicVFF, CAbstractHolonomicReactiveMethod, mrpt::nav)
 
/*---------------------------------------------------------------
                                                initialize
  ---------------------------------------------------------------*/
CHolonomicVFF::CHolonomicVFF(const mrpt::config::CConfigFileBase* INI_FILE)
        : CAbstractHolonomicReactiveMethod("CHolonomicVFF")
{
        if (INI_FILE != nullptr) initialize(*INI_FILE);
}
 
void CHolonomicVFF::initialize(const mrpt::config::CConfigFileBase& INI_FILE)
{
        options.loadFromConfigFile(INI_FILE, getConfigFileSectionName());
}
void CHolonomicVFF::saveConfigFile(mrpt::config::CConfigFileBase& c) const
{
        options.saveToConfigFile(c, getConfigFileSectionName());
}
 
/*---------------------------------------------------------------
                                                navigate
  ---------------------------------------------------------------*/
void CHolonomicVFF::navigate(const NavInput& ni, NavOutput& no)
{
        const auto ptg = getAssociatedPTG();
        const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;
 
        // Create a log record for returning data.
        no.logRecord = mrpt::make_aligned_shared<CLogFileRecord_VFF>();
 
        // Forces vector:
        mrpt::math::TPoint2D resultantForce(0, 0), instantaneousForce(0, 0);
 
        // Obstacles:
        {
                const size_t n = ni.obstacles.size();
                const double inc_ang = 2 * M_PI / n;
                double ang = -M_PI + 0.5 * inc_ang;
                for (size_t i = 0; i < n; i++, ang += inc_ang)
                {
                        // Compute force strength:
                        // const double mod = exp(- obstacles[i] );
                        const double mod = std::min(1e6, 1.0 / ni.obstacles[i]);
 
                        // Add repulsive force:
                        instantaneousForce.x = -cos(ang) * mod;
                        instantaneousForce.y = -sin(ang) * mod;
                        resultantForce += instantaneousForce;
                }
        }
 
        const double obstcl_weight = 20.0 / ni.obstacles.size();
        resultantForce *= obstcl_weight;
 
        const double obstacleNearnessFactor =
                std::min(1.0, 6.0 / resultantForce.norm());
 
        // Target:
        ASSERT_(!ni.targets.empty());
        const auto trg = *ni.targets.rbegin();
 
        const double ang = atan2(trg.y, trg.x);
        const double mod = options.TARGET_ATTRACTIVE_FORCE;
        resultantForce += mrpt::math::TPoint2D(cos(ang) * mod, sin(ang) * mod);
 
        // Result:
        no.desiredDirection = (resultantForce.y == 0 && resultantForce.x == 0)
                                                          ? 0
                                                          : atan2(resultantForce.y, resultantForce.x);
 
        // Speed control: Reduction factors
        // ---------------------------------------------
        if (m_enableApproachTargetSlowDown)
        {
                const double targetNearnessFactor = std::min(
                        1.0, trg.norm() /
                                         (options.TARGET_SLOW_APPROACHING_DISTANCE / ptg_ref_dist));
                no.desiredSpeed =
                        ni.maxRobotSpeed *
                        std::min(obstacleNearnessFactor, targetNearnessFactor);
        }
}
 
uint8_t CHolonomicVFF::serializeGetVersion() const { return 0; }
void CHolonomicVFF::serializeTo(mrpt::serialization::CArchive& out) const
{
        out << options.TARGET_ATTRACTIVE_FORCE
                << options.TARGET_SLOW_APPROACHING_DISTANCE;
}
void CHolonomicVFF::serializeFrom(
        mrpt::serialization::CArchive& in, uint8_t version)
{
        switch (version)
        {
                case 0:
                {
                        in >> options.TARGET_ATTRACTIVE_FORCE >>
                                options.TARGET_SLOW_APPROACHING_DISTANCE;
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}
 
uint8_t CLogFileRecord_VFF::serializeGetVersion() const { return 0; }
void CLogFileRecord_VFF::serializeTo(mrpt::serialization::CArchive& out) const
{
}
 
void CLogFileRecord_VFF::serializeFrom(
        mrpt::serialization::CArchive& in, uint8_t version)
{
        MRPT_UNUSED_PARAM(in);
        switch (version)
        {
                case 0:
                {
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}
 
/*---------------------------------------------------------------
                                                TOptions
  ---------------------------------------------------------------*/
CHolonomicVFF::TOptions::TOptions()
        : TARGET_SLOW_APPROACHING_DISTANCE(0.10), TARGET_ATTRACTIVE_FORCE(20.0)
{
}
 
void CHolonomicVFF::TOptions::loadFromConfigFile(
        const mrpt::config::CConfigFileBase& source, const std::string& section)
{
        MRPT_START
 
        // Load from config text:
        MRPT_LOAD_CONFIG_VAR(
                TARGET_SLOW_APPROACHING_DISTANCE, double, source, section);
        MRPT_LOAD_CONFIG_VAR(TARGET_ATTRACTIVE_FORCE, double, source, section);
 
        MRPT_END
}
 
void CHolonomicVFF::TOptions::saveToConfigFile(
        mrpt::config::CConfigFileBase& c, const std::string& s) const
{
        MRPT_START;
 
        MRPT_SAVE_CONFIG_VAR_COMMENT(
                TARGET_SLOW_APPROACHING_DISTANCE, "For stopping gradually");
        MRPT_SAVE_CONFIG_VAR_COMMENT(
                TARGET_ATTRACTIVE_FORCE,
                "Dimension-less (may have to be tuned depending on the density of "
                "obstacle sampling)");
 
        MRPT_END;
}