ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
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 }