ClearanceDiagram.h

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 |
   +------------------------------------------------------------------------+ */
#pragma once

#include <vector>
#include <map>
#include <cstdint>
#include <mrpt/opengl/opengl_frwds.h>
namespace mrpt
{
namespace utils
{
class CStream;
}
}

namespace mrpt
{
namespace nav
{
/** Clearance information for one particular PTG and one set of obstacles.
        * Usage:
        * - Declare an object of this type (it will be initialized to "empty"),
        * - Call CParameterizedTrajectoryGenerator::initClearanceDiagram()
        * - Repeatedly call CParameterizedTrajectoryGenerator::updateClearance() for
 * each 2D obstacle point.
        *
        *  \ingroup nav_tpspace
        */
class ClearanceDiagram
{
   public:
        /** default ctor */
        ClearanceDiagram();
        /** Reset to default, empty state */
        void clear();
        /** Initializes the container to allocate `decimated_num_paths` entries, as
         * a decimated
          * subset of a total of `actual_num_paths` paths */
        void resize(size_t actual_num_paths, size_t decimated_num_paths);
        inline bool empty() const { return m_raw_clearances.empty(); }
        inline size_t get_actual_num_paths() const { return m_actual_num_paths; }
        inline size_t get_decimated_num_paths() const
        {
                return m_raw_clearances.size();
        }

        /** Gets the clearance for path `k` and distance `TPS_query_distance` in one
         * of two modes:
          * - [integrate_over_path=false] clearance from that specific spot, or
          * - [integrate_over_path=true] average clearance over the path from the
         * origin to that specific spot.
          */
        double getClearance(
                uint16_t k, double TPS_query_distance, bool integrate_over_path) const;
        void renderAs3DObject(
                mrpt::opengl::CMesh& mesh, double min_x, double max_x, double min_y,
                double max_y, double cell_res, bool integrate_over_path) const;

        void readFromStream(mrpt::utils::CStream& in);
        void writeToStream(mrpt::utils::CStream& out) const;

        /** [TPS_distance] => normalized_clearance_for_exactly_that_robot_pose  */
        typedef std::map<double, double> dist2clearance_t;
        dist2clearance_t& get_path_clearance(size_t actual_k);
        const dist2clearance_t& get_path_clearance(size_t actual_k) const;

        inline dist2clearance_t& get_path_clearance_decimated(size_t decim_k)
        {
                return m_raw_clearances[decim_k];
        }
        inline const dist2clearance_t& get_path_clearance_decimated(
                size_t decim_k) const
        {
                return m_raw_clearances[decim_k];
        }

        size_t real_k_to_decimated_k(size_t k) const;
        size_t decimated_k_to_real_k(size_t k) const;

   protected:
        /** Container: [decimated_path_k][TPS_distance] =>
         * normalized_clearance_for_exactly_that_robot_pose  */
        std::vector<dist2clearance_t> m_raw_clearances;

        size_t m_actual_num_paths;  // The decimated number of paths is implicit in
        // raw_clearances.size()
        double m_k_a2d, m_k_d2a;
};

}  // ns
}  // ns