MapGrid2D.h

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
#ifndef YARP_DEV_MAPGRID2D_H
#define YARP_DEV_MAPGRID2D_H
 
#include <yarp/dev/api.h>
 
#include <yarp/os/ConnectionReader.h>
#include <yarp/os/Portable.h>
 
#include <yarp/sig/Image.h>
 
#include <yarp/dev/MapGrid2DInfo.h>
 
#include <yarp/math/Vec2D.h>
 
#include <string>
 
namespace yarp::dev::Nav2D {
class YARP_dev_API MapGrid2D : public yarp::os::Portable, public yarp::dev::Nav2D::MapGrid2DInfo
{
public:
    typedef yarp::sig::PixelMono CellFlagData;
    typedef yarp::sig::PixelMono CellOccupancyData;
    // typedef yarp::math::Vec2D<int> XYCell;
    // typedef yarp::math::Vec2D<double> XYWorld;
 
    enum map_flags
    {
        MAP_CELL_FREE = 0,
        MAP_CELL_KEEP_OUT = 1,
        MAP_CELL_TEMPORARY_OBSTACLE = 2,
        MAP_CELL_ENLARGED_OBSTACLE = 3,
        MAP_CELL_WALL = 4,
        MAP_CELL_UNKNOWN = 5
    };
 
private:
    // those two always have the same size
    yarp::sig::ImageOf<CellOccupancyData> m_map_occupancy;
    yarp::sig::ImageOf<CellFlagData> m_map_flags;
 
    double m_occupied_thresh;
    double m_free_thresh;
 
    // std::vector<map_link> links_to_other_maps;
 
private:
    bool m_compressed_data_over_network;
 
public:
    bool enable_map_compression_over_network(bool val);
 
private:
    // performs an obstacles enlargement on the specified cell.
    void enlargeCell(XYCell cell);
 
    // conversion from pixel color to CellFlagData (yarp format) and viceversa
    CellFlagData PixelToCellFlagData(const yarp::sig::PixelRgb& pixin) const;
    yarp::sig::PixelRgb CellFlagDataToPixel(const CellFlagData& cellin) const;
 
    // conversion from pixel color to CellOccupancyData (occupancy grid, ros format) and viceversa
    CellOccupancyData PixelToCellOccupancyData(const yarp::sig::PixelMono& pixin) const;
    yarp::sig::PixelMono CellOccupancyDataToPixel(const CellOccupancyData& cellin) const;
 
    // internal methods to read a map from file, either in yarp or ROS format
    bool loadMapYarpOnly(std::string yarp_img_filename);
    bool loadMapROSOnly(std::string ros_yaml_filename);
    bool loadROSParams(std::string ros_yaml_filename, std::string& pgm_occ_filename, double& resolution, double& orig_x, double& orig_y, double& orig_t);
    bool loadMapYarpAndRos(std::string yarp_img_filename, std::string ros_yaml_filename);
    bool parseMapParameters(const yarp::os::Property& mapfile);
 
public:
    MapGrid2D();
    virtual ~MapGrid2D();
 
    bool isWall(XYCell cell) const;
 
    bool isFree(XYCell cell) const;
 
    bool isNotFree(XYCell cell) const;
 
    bool isKeepOut(XYCell cell) const;
 
    bool getMapFlag(XYCell cell, map_flags& flag) const;
 
    bool setMapFlag(XYCell cell, map_flags flag);
 
    void clearMapTemporaryFlags();
 
    bool setOccupancyData(XYCell cell, double occupancy);
 
    bool getOccupancyData(XYCell cell, double& occupancy) const;
 
    bool setMapImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>& image);
    bool getMapImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>& image) const;
    bool setOccupancyGrid(yarp::sig::ImageOf<yarp::sig::PixelMono>& image);
    bool getOccupancyGrid(yarp::sig::ImageOf<yarp::sig::PixelMono>& image) const;
 
    bool setOrigin(double x, double y, double theta);
 
    void getOrigin(double& x, double& y, double& theta) const;
 
    bool setResolution(double resolution);
 
    void getResolution(double& resolution) const;
 
    bool setSize_in_meters(double x, double y);
 
    bool setSize_in_cells(size_t x, size_t y);
 
    void getSize_in_meters(double& x, double& y) const;
 
    void getSize_in_cells(size_t& x, size_t& y) const;
 
    size_t width() const;
 
    size_t height() const;
 
    bool setMapName(std::string map_name);
 
    std::string getMapName() const;
 
    //------------------------------utility functions-------------------------------
 
    bool crop(int left, int top, int right, int bottom);
 
#if 0
    bool   isInsideMap(XYCell cell) const;
 
    bool   isInsideMap(XYWorld world) const;
#endif
 
    bool isIdenticalTo(const MapGrid2D& otherMap) const;
 
    bool enlargeObstacles(double size);
 
    //-------------------------------file access functions-------------------------------
 
    bool loadFromFile(std::string map_filename);
 
    bool saveToFile(std::string map_filename) const;
 
    /*
     * Read vector from a connection.
     * return true iff a vector was read correctly
     */
    bool read(yarp::os::ConnectionReader& connection) override;
 
    bool write(yarp::os::ConnectionWriter& connection) const override;
};
} // namespace yarp::dev::Nav2D
 
#endif // YARP_DEV_MAPGRID2D_H