WorldMap.hpp

Go to the documentation of this file.

#pragma once
 
#include "libstp/map/Geometry.hpp"
#include "libstp/map/MapSegment.hpp"
 
#include <filesystem>
#include <stdexcept>
#include <string>
#include <vector>
 
namespace libstp::map
{
    class FtmapParseError : public std::runtime_error
    {
    public:
        using std::runtime_error::runtime_error;
    };

    struct MapLayer
    {
        std::string id;
        std::string name;
        float zCm{0.0f};
        std::vector<MapSegment> segments;
    };

    struct TransitionEdge
    {
        float startX{0.0f};
        float startY{0.0f};
        float endX{0.0f};
        float endY{0.0f};
    };

    struct MapTransition
    {
        std::string id;
        std::string name;
        std::string fromLayer;
        std::string toLayer;
        TransitionEdge from;
        TransitionEdge to;
        bool bidirectional{true};
        float widthCm{0.0f};
    };

    class WorldMap
    {
    public:
        WorldMap() = default;
 
        // ───────────── Bounds + segment access ─────────────
        float tableWidthCm() const noexcept { return m_tableWidthCm; }
        float tableHeightCm() const noexcept { return m_tableHeightCm; }
        const std::vector<MapSegment>& segments() const noexcept { return m_segments; }
 
        // ───────────── Multi-layer (v2) access ─────────────
        // v1 maps load as a single "ground" layer; v2 maps load all authored
        // layers. `segments()` and the no-arg queries always operate on the
        // ground/active layer (m_segments) so the particle filter and every
        // pre-v2 caller keep working unchanged. SimWorld uses the layer-indexed
        // overloads with the plane the robot is currently on.
 
        std::size_t layerCount() const noexcept { return m_layers.size(); }
        const std::vector<MapLayer>& layers() const noexcept { return m_layers; }
        const std::vector<MapTransition>& transitions() const noexcept { return m_transitions; }

        int layerIndex(const std::string& id) const noexcept;

        const std::vector<MapSegment>& layerSegments(std::size_t idx) const noexcept;

        bool isOnLine(float xCm, float yCm, std::size_t layerIdx) const;
        bool isOnWall(float xCm, float yCm, std::size_t layerIdx) const;

        std::vector<MapSegment> lines() const;

        std::vector<MapSegment> walls() const;
 
        // ───────────── Point queries ─────────────

        bool isOnBlackLine(float xCm, float yCm) const;

        bool isOnLine(float xCm, float yCm) const;

        bool isOnWall(float xCm, float yCm) const;

        float distanceToNearestLine(float xCm, float yCm) const;

        float distanceToNearestWall(float xCm, float yCm) const;
 
        // ───────────── Sensor projection ─────────────

        Point2D sensorFieldPosition(float robotXCm,
                                    float robotYCm,
                                    float robotHeadingRad,
                                    const SensorOffset& sensor) const;

        bool sensorIsOnLine(float robotXCm,
                            float robotYCm,
                            float robotHeadingRad,
                            const SensorOffset& sensor) const;

        bool sensorIsOnWall(float robotXCm,
                            float robotYCm,
                            float robotHeadingRad,
                            const SensorOffset& sensor) const;
 
        // ───────────── Loading ─────────────

        void loadFtmap(const std::filesystem::path& path);

        void parseFtmap(const std::string& content);
 
        // ───────────── Programmatic construction ─────────────
        void setTable(float widthCm, float heightCm) noexcept
        {
            m_tableWidthCm = widthCm;
            m_tableHeightCm = heightCm;
        }
        void addSegment(const MapSegment& seg) { m_segments.push_back(seg); }
        void clear() noexcept
        {
            m_segments.clear();
            m_layers.clear();
            m_transitions.clear();
            m_tableWidthCm = 0.0f;
            m_tableHeightCm = 0.0f;
        }
 
    private:
        float m_tableWidthCm{0.0f};
        float m_tableHeightCm{0.0f};
        // Ground/active layer segments — backs segments() and the no-arg
        // queries. Mirrors m_layers[ground] so legacy callers are unaffected.
        std::vector<MapSegment> m_segments;
        std::vector<MapLayer> m_layers;
        std::vector<MapTransition> m_transitions;
    };
}