CPTG_RobotShape_Circular.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/CParameterizedTrajectoryGenerator.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/utils/round.h>
 
using namespace mrpt::nav;
 
CPTG_RobotShape_Circular::CPTG_RobotShape_Circular() : m_robotRadius(.0) {}
CPTG_RobotShape_Circular::~CPTG_RobotShape_Circular() {}
void CPTG_RobotShape_Circular::setRobotShapeRadius(const double robot_radius)
{
        m_robotRadius = robot_radius;
        internal_processNewRobotShape();
}
 
void CPTG_RobotShape_Circular::loadDefaultParams() { m_robotRadius = 0.2; }
void CPTG_RobotShape_Circular::loadShapeFromConfigFile(
        const mrpt::utils::CConfigFileBase& cfg, const std::string& sSection)
{
        const double old_R = m_robotRadius;
        MRPT_LOAD_HERE_CONFIG_VAR(
                robot_radius, double, m_robotRadius, cfg, sSection);
 
        if (m_robotRadius != old_R) internal_processNewRobotShape();
}
void CPTG_RobotShape_Circular::saveToConfigFile(
        mrpt::utils::CConfigFileBase& cfg, const std::string& sSection) const
{
        const int WN = 25, WV = 30;
 
        cfg.write(
                sSection, "robot_radius", m_robotRadius, WN, WV, "Robot radius [m].");
}
 
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 = 17;
        // 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]);
        }
        // Draw a "cross" to identify the robot center
        gl_shape.appendLine(
                origin.x() - R * 0.02, origin.y(), .0, origin.x() + R * 0.02,
                origin.y(), .0);
        gl_shape.appendLine(
                origin.x(), origin.y() - R * 0.02, .0, origin.x(),
                origin.y() + R * 0.02, .0);
}
 
void CPTG_RobotShape_Circular::internal_shape_loadFromStream(
        mrpt::utils::CStream& in)
{
        uint8_t version;
        in >> version;
 
        switch (version)
        {
                case 0:
                        in >> m_robotRadius;
                        break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
        }
}
 
void CPTG_RobotShape_Circular::internal_shape_saveToStream(
        mrpt::utils::CStream& out) const
{
        uint8_t version = 0;
        out << version;
 
        out << m_robotRadius;
}
 
double CPTG_RobotShape_Circular::getMaxRobotRadius() const
{
        return m_robotRadius;
}
 
bool CPTG_RobotShape_Circular::isPointInsideRobotShape(
        const double x, const double y) const
{
        return mrpt::math::hypot_fast(x, y) < m_robotRadius;
}
 
double CPTG_RobotShape_Circular::evalClearanceToRobotShape(
        const double ox, const double oy) const
{
        return mrpt::math::hypot_fast(ox, oy) - m_robotRadius;
}