MapGrid2D.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms of the
 * BSD-3-Clause license. See the accompanying LICENSE file for details.
 */
 
#ifndef YARP_DEV_MAPGRID2D_H
#define YARP_DEV_MAPGRID2D_H
 
#include <string>
 
#include <yarp/os/Portable.h>
#include <yarp/os/ConnectionReader.h>
#include <yarp/sig/Image.h>
#include <yarp/math/Vec2D.h>
#include <yarp/dev/api.h>
#include <yarp/dev/MapGrid2DInfo.h>
 
namespace yarp
{
    namespace dev
    {
        namespace Nav2D
        {
            class  YARP_dev_API MapGrid2D : public yarp::os::Portable,
                                            public yarp::dev::Nav2D::MapGrid2DInfo
            {
            public:
                typedef yarp::sig::PixelMono CellData;
                //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<CellData> m_map_occupancy;
                yarp::sig::ImageOf<CellData> m_map_flags;
 
                double m_occupied_thresh;
                double m_free_thresh;
 
                //std::vector<map_link> links_to_other_maps;
 
            private:
                //performs an obstacles enlargement on the specified cell.
                void enlargeCell(XYCell cell);
 
                //conversion from pixel color to CellData and viceversa
                CellData PixelToCellData(const yarp::sig::PixelRgb& pixin) const;
                yarp::sig::PixelRgb CellDataToPixel(const CellData& pixin) 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);
                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;
            };
        }
    }
}
 
#endif // YARP_DEV_MAPGRID2D_H