ARGoS  3
A parallel, multi-engine simulator for swarm robotics
core/simulator/entity/floor_entity.cpp
Go to the documentation of this file.
00001 
00007 #include "floor_entity.h"
00008 #include <FreeImagePlus.h>
00009 #include <argos3/core/simulator/simulator.h>
00010 #include <argos3/core/simulator/space/space.h>
00011 #include <argos3/core/simulator/loop_functions.h>
00012 
00013 namespace argos {
00014 
00015       /****************************************/
00016       /****************************************/
00017 
00018    class CFloorColorFromImageFile : public CFloorEntity::CFloorColorSource {
00019 
00020    public:
00021 
00022       CFloorColorFromImageFile(const std::string& str_path) {
00023          const CVector3& cArenaSize = CSimulator::GetInstance().GetSpace().GetArenaSize();
00024          m_cHalfArenaSize.Set(
00025             cArenaSize.GetX() * 0.5f,
00026             cArenaSize.GetY() * 0.5f);
00027          LoadImage(str_path);
00028       }
00029 
00030       virtual void Reset() {
00031          LoadImage(m_strImageFileName);
00032       }
00033 
00034       virtual CColor GetColorAtPoint(Real f_x,
00035                                      Real f_y) {
00036          /* Compute coordinates on the image */
00037          UInt32 x = (f_x + m_cHalfArenaSize.GetX()) * m_fArenaToImageCoordinateXFactor;
00038          UInt32 y = (f_y + m_cHalfArenaSize.GetY()) * m_fArenaToImageCoordinateYFactor;
00039          /* Check the bit depth */
00040          if(m_cImage.getBitsPerPixel() <= 8) {
00041             RGBQUAD* ptColorPalette;
00042             BYTE tPixelIndex;
00043             /* 1, 4 or 8 bits per pixel */
00044             if(! m_cImage.getPixelIndex(x, y, &tPixelIndex)) {
00045                THROW_ARGOSEXCEPTION("Unable to access image pixel at (" << x << "," << y <<
00046                                     "). Image size (" << m_cImage.getWidth() << "," <<
00047                                     m_cImage.getHeight() << ")");
00048             }
00049             ptColorPalette = m_cImage.getPalette();
00050             return CColor(ptColorPalette[tPixelIndex].rgbRed,
00051                           ptColorPalette[tPixelIndex].rgbGreen,
00052                           ptColorPalette[tPixelIndex].rgbBlue);
00053          }
00054          else {
00055             /* 16, 24 or 32 bits per pixel */
00056             RGBQUAD tColorPixel;
00057             if(! m_cImage.getPixelColor(x, y, &tColorPixel)) {
00058                THROW_ARGOSEXCEPTION("Unable to access image pixel at (" << x << "," << y <<
00059                                     "). Image size (" << m_cImage.getWidth() << "," <<
00060                                     m_cImage.getHeight() << ")");
00061             }
00062             return CColor(tColorPixel.rgbRed,
00063                           tColorPixel.rgbGreen,
00064                           tColorPixel.rgbBlue);
00065          }
00066       }
00067 
00068       virtual void SaveAsImage(const std::string& str_path) {
00069          m_strImageFileName = str_path;
00070          m_cImage.save(str_path.c_str());
00071       }
00072 
00073       virtual const std::string& GetImageFileName() const {
00074          return m_strImageFileName;
00075       }
00076 
00077    protected:
00078 
00079       void LoadImage(const std::string& str_path) {
00080          m_strImageFileName = str_path;
00081          if(!m_cImage.load(m_strImageFileName.c_str())) {
00082             THROW_ARGOSEXCEPTION("Could not load image \"" <<
00083                                  m_strImageFileName <<
00084                                  "\"");
00085          }
00086          const CVector3& cArenaSize = CSimulator::GetInstance().GetSpace().GetArenaSize();
00087          m_fArenaToImageCoordinateXFactor = m_cImage.getWidth() / cArenaSize.GetX();
00088          m_fArenaToImageCoordinateYFactor = m_cImage.getHeight() / cArenaSize.GetY();
00089       }
00090 
00091    private:
00092 
00093       fipImage m_cImage;
00094       Real m_fArenaToImageCoordinateXFactor;
00095       Real m_fArenaToImageCoordinateYFactor;
00096       CVector2 m_cHalfArenaSize;
00097       std::string m_strImageFileName;
00098 
00099    };
00100 
00101       /****************************************/
00102       /****************************************/
00103 
00104    class CFloorColorFromLoopFunctions : public CFloorEntity::CFloorColorSource {
00105 
00106    public:
00107 
00108       CFloorColorFromLoopFunctions(UInt32 un_pixels_per_meter) :
00109          m_cLoopFunctions(CSimulator::GetInstance().GetLoopFunctions()),
00110          m_unPixelsPerMeter(un_pixels_per_meter) {
00111          const CVector3& cArenaSize = CSimulator::GetInstance().GetSpace().GetArenaSize();
00112          m_cHalfArenaSize.Set(
00113             cArenaSize.GetX() * 0.5f,
00114             cArenaSize.GetY() * 0.5f);
00115       }
00116 
00117       virtual CColor GetColorAtPoint(Real f_x,
00118                                      Real f_y) {
00119          return m_cLoopFunctions.GetFloorColor(CVector2(f_x, f_y));
00120       }
00121 
00122       virtual void SaveAsImage(const std::string& str_path) {
00123          fipImage cImage(FIT_BITMAP, m_unPixelsPerMeter * m_cHalfArenaSize.GetX()*2, m_unPixelsPerMeter * m_cHalfArenaSize.GetY()*2, 24);
00124          Real fFactor = 1.0f / static_cast<Real>(m_unPixelsPerMeter);
00125          CVector2 cFloorPos;
00126          CColor cARGoSPixel;
00127          RGBQUAD tFIPPixel;
00128          for(UInt32 y = 0; y < cImage.getHeight(); ++y) {
00129             for(UInt32 x = 0; x < cImage.getWidth(); ++x) {
00130                cFloorPos.Set(x * fFactor, y * fFactor);
00131                cFloorPos -= m_cHalfArenaSize;
00132                cARGoSPixel = m_cLoopFunctions.GetFloorColor(cFloorPos);
00133                tFIPPixel.rgbRed = cARGoSPixel.GetRed();
00134                tFIPPixel.rgbGreen = cARGoSPixel.GetGreen();
00135                tFIPPixel.rgbBlue = cARGoSPixel.GetBlue();
00136                cImage.setPixelColor(x, y, &tFIPPixel);
00137             }
00138          }
00139          if(!cImage.save(str_path.c_str())) {
00140             THROW_ARGOSEXCEPTION("Cannot save image \"" << str_path << "\" for floor entity.");
00141          }
00142       }
00143 
00144    private:
00145 
00146       CLoopFunctions& m_cLoopFunctions;
00147       UInt32 m_unPixelsPerMeter;
00148       CVector2 m_cHalfArenaSize;
00149    };
00150 
00151    /****************************************/
00152    /****************************************/
00153 
00154    CFloorEntity::CFloorEntity() :
00155       CEntity(NULL),
00156       m_eColorSource(UNSET),
00157       m_pcColorSource(NULL),
00158       m_bHasChanged(true) {}
00159 
00160    /****************************************/
00161    /****************************************/
00162 
00163    CFloorEntity::CFloorEntity(const std::string& str_id,
00164                               const std::string& str_file_name) :
00165       CEntity(NULL, str_id),
00166       m_eColorSource(FROM_IMAGE),
00167       m_pcColorSource(NULL),
00168       m_bHasChanged(true) {
00169       std::string strFileName = str_file_name;
00170       ExpandEnvVariables(strFileName);
00171       m_pcColorSource = new CFloorColorFromImageFile(strFileName);
00172    }
00173 
00174    /****************************************/
00175    /****************************************/
00176 
00177    CFloorEntity::CFloorEntity(const std::string& str_id,
00178                               UInt32 un_pixels_per_meter) :
00179       CEntity(NULL, str_id),
00180       m_eColorSource(FROM_LOOP_FUNCTIONS),
00181       m_pcColorSource(new CFloorColorFromLoopFunctions(un_pixels_per_meter)),
00182       m_bHasChanged(true) {}
00183 
00184    /****************************************/
00185    /****************************************/
00186 
00187    CFloorEntity::~CFloorEntity() {
00188       if(m_pcColorSource != NULL) {
00189          delete m_pcColorSource;
00190       }
00191    }
00192 
00193    /****************************************/
00194    /****************************************/
00195 
00196    void CFloorEntity::Init(TConfigurationNode& t_tree) {
00197       /* Init parent */
00198       CEntity::Init(t_tree);
00199       /* Parse XML */
00200       std::string strColorSource;
00201       GetNodeAttribute(t_tree, "source", strColorSource);
00202       if(strColorSource == "image") {
00203          m_eColorSource = FROM_IMAGE;
00204          std::string strPath;
00205          GetNodeAttribute(t_tree, "path", strPath);
00206          ExpandEnvVariables(strPath);
00207          m_pcColorSource = new CFloorColorFromImageFile(strPath);
00208       }
00209       else if(strColorSource == "loop_functions") {
00210          m_eColorSource = FROM_LOOP_FUNCTIONS;
00211          UInt32 unPixelsPerMeter;
00212          GetNodeAttribute(t_tree, "pixels_per_meter", unPixelsPerMeter);
00213          m_pcColorSource = new CFloorColorFromLoopFunctions(unPixelsPerMeter);
00214       }
00215       else {
00216          THROW_ARGOSEXCEPTION("Unknown image source \"" <<
00217                               strColorSource <<
00218                               "\" for the floor entity \"" <<
00219                               GetId() <<
00220                               "\"");
00221       }
00222    }
00223 
00224    /****************************************/
00225    /****************************************/
00226 
00227    void CFloorEntity::Reset() {
00228       m_pcColorSource->Reset();
00229    }
00230 
00231    /****************************************/
00232    /****************************************/
00233 
00234    REGISTER_ENTITY(CFloorEntity,
00235                    "floor",
00236                    "Carlo Pinciroli [ilpincy@gmail.com]",
00237                    "1.0",
00238                    "It contains the properties of the arena floor.",
00239                    "The floor entity contains the properties of the arena floor. In the current\n"
00240                    "implementation, it contains only the color of the floor. The floor color is\n"
00241                    "detected by the robots' ground sensors.\n\n"
00242                    "REQUIRED XML CONFIGURATION\n\n"
00243                    "  <arena ...>\n"
00244                    "    ...\n"
00245                    "    <floor id=\"floor\"\n"
00246                    "           source=\"SOURCE\" />\n"
00247                    "    ...\n"
00248                    "  </arena>\n\n"
00249                    "The 'id' attribute is necessary and must be unique among the entities. If two\n"
00250                    "entities share the same id, initialization aborts.\n"
00251                    "The 'source' attribute specifies where to get the color of the floor from. Its\n"
00252                    "value, here denoted as SOURCE, can assume the following values:\n\n"
00253                    "  image            The color is calculated from the passed image file\n"
00254                    "  loop_functions   The color is calculated calling the loop functions\n\n"
00255                    "When 'source' is set to 'image', as showed in the following example, you have\n"
00256                    "to specify the image path in the additional attribute 'path':\n\n"
00257                    "  <arena ...>\n"
00258                    "    ...\n"
00259                    "    <floor id=\"floor\"\n"
00260                    "           source=\"image\"\n"
00261                    "           path=\"/path/to/imagefile.ext\" />\n"
00262                    "    ...\n"
00263                    "  </arena>\n\n"
00264                    "Many image formats are available, such as PNG, JPG, BMP, GIF and many more.\n"
00265                    "Refer to the FreeImage webpage for a complete list of supported image formats\n"
00266                    "(http://freeimage.sourceforge.net/features.html).\n\n"
00267                    "When 'source' is set to 'loop_functions', as showed in the following example,\n"
00268                    "an image is implicitly created to be used as texture for graphical\n"
00269                    "visualizations. The algorithm that creates the texture needs to convert from\n"
00270                    "meters (in the arena) to pixels (of the texture). You control how many pixels\n"
00271                    "per meter are used with the attribute 'pixels_per_meter'. Clearly, the higher\n"
00272                    "value, the higher the quality, but also the slower the algorithm and the bigger\n"
00273                    "the texture. The algorithm is called only once at init time, so the fact that\n"
00274                    "it is slow is not so important. However, the image size is limited by OpenGL.\n"
00275                    "Every implementation has its own limit, and you should check yours if any\n"
00276                    "texture-related problem arises. Now for the example:\n\n"
00277                    "  <arena ...>\n"
00278                    "    ...\n"
00279                    "    <floor id=\"floor\"\n"
00280                    "           source=\"loop_functions\"\n"
00281                    "           pixels_per_meter=\"100\" />\n"
00282                    "    ...\n"
00283                    "  </arena>\n\n"
00284                    "OPTIONAL XML CONFIGURATION\n\n"
00285                    "None for the time being.\n",
00286                    "Usable"
00287       );
00288 
00289    /****************************************/
00290    /****************************************/
00291 
00292    class CSpaceOperationAddCFloorEntity : public CSpaceOperationAddEntity {
00293    public:
00294       void ApplyTo(CSpace& c_space, CFloorEntity& c_entity) {
00295          c_space.AddEntity(c_entity);
00296          c_space.SetFloorEntity(c_entity);
00297       }
00298    };
00299 
00300    REGISTER_SPACE_OPERATION(CSpaceOperationAddEntity,
00301                             CSpaceOperationAddCFloorEntity,
00302                             CFloorEntity);
00303    REGISTER_STANDARD_SPACE_OPERATION_REMOVE_ENTITY(CFloorEntity);
00304 
00305    /****************************************/
00306    /****************************************/
00307 
00308 }