floor_entity.cpp
Go to the documentation of this file.
1 
7 #include "floor_entity.h"
8 #include <argos3/core/simulator/simulator.h>
9 #include <argos3/core/simulator/space/space.h>
10 #include <argos3/core/simulator/loop_functions.h>
11 
12 #ifdef ARGOS_WITH_FREEIMAGE
13 #include <FreeImagePlus.h>
14 #endif
15 
16 namespace argos {
17 
18  /****************************************/
19  /****************************************/
20 
21 #ifdef ARGOS_WITH_FREEIMAGE
22  class CFloorColorFromImageFile : public CFloorEntity::CFloorColorSource {
23 
24  public:
25 
26  CFloorColorFromImageFile(const std::string& str_path) {
27  const CVector3& cArenaSize = CSimulator::GetInstance().GetSpace().GetArenaSize();
28  m_cHalfArenaSize.Set(
29  cArenaSize.GetX() * 0.5f,
30  cArenaSize.GetY() * 0.5f);
31  const CVector3& cArenaCenter = CSimulator::GetInstance().GetSpace().GetArenaCenter();
32  m_cArenaCenter.Set(cArenaCenter.GetX(),
33  cArenaCenter.GetY());
34  LoadImage(str_path);
35  }
36 
37  virtual void Reset() {
38  LoadImage(m_strImageFileName);
39  }
40 
41  virtual CColor GetColorAtPoint(Real f_x,
42  Real f_y) {
43  /* Compute coordinates on the image */
44  UInt32 x = (f_x + m_cHalfArenaSize.GetX()) * m_fArenaToImageCoordinateXFactor;
45  UInt32 y = (f_y + m_cHalfArenaSize.GetY()) * m_fArenaToImageCoordinateYFactor;
46  /* Check the bit depth */
47  if(m_cImage.getBitsPerPixel() <= 8) {
48  RGBQUAD* ptColorPalette;
49  BYTE tPixelIndex;
50  /* 1, 4 or 8 bits per pixel */
51  if(! m_cImage.getPixelIndex(x, y, &tPixelIndex)) {
52  THROW_ARGOSEXCEPTION("Unable to access image pixel at (" << x << "," << y <<
53  "). Image size (" << m_cImage.getWidth() << "," <<
54  m_cImage.getHeight() << ")");
55  }
56  ptColorPalette = m_cImage.getPalette();
57  return CColor(ptColorPalette[tPixelIndex].rgbRed,
58  ptColorPalette[tPixelIndex].rgbGreen,
59  ptColorPalette[tPixelIndex].rgbBlue);
60  }
61  else {
62  /* 16, 24 or 32 bits per pixel */
63  RGBQUAD tColorPixel;
64  if(! m_cImage.getPixelColor(x, y, &tColorPixel)) {
65  THROW_ARGOSEXCEPTION("Unable to access image pixel at (" << x << "," << y <<
66  "). Image size (" << m_cImage.getWidth() << "," <<
67  m_cImage.getHeight() << ")");
68  }
69  return CColor(tColorPixel.rgbRed,
70  tColorPixel.rgbGreen,
71  tColorPixel.rgbBlue);
72  }
73  }
74 
75  virtual void SaveAsImage(const std::string& str_path) {
76  m_strImageFileName = str_path;
77  m_cImage.save(str_path.c_str());
78  }
79 
80  virtual const std::string& GetImageFileName() const {
81  return m_strImageFileName;
82  }
83 
84  protected:
85 
86  void LoadImage(const std::string& str_path) {
87  m_strImageFileName = str_path;
88  if(!m_cImage.load(m_strImageFileName.c_str())) {
89  THROW_ARGOSEXCEPTION("Could not load image \"" <<
90  m_strImageFileName <<
91  "\"");
92  }
93  const CVector3& cArenaSize = CSimulator::GetInstance().GetSpace().GetArenaSize();
94  m_fArenaToImageCoordinateXFactor = m_cImage.getWidth() / cArenaSize.GetX();
95  m_fArenaToImageCoordinateYFactor = m_cImage.getHeight() / cArenaSize.GetY();
96  }
97 
98  private:
99 
100  fipImage m_cImage;
101  Real m_fArenaToImageCoordinateXFactor;
102  Real m_fArenaToImageCoordinateYFactor;
103  CVector2 m_cHalfArenaSize;
104  CVector2 m_cArenaCenter;
105  std::string m_strImageFileName;
106 
107  };
108 #endif
109 
110  /****************************************/
111  /****************************************/
112 
114 
115  public:
116 
117  CFloorColorFromLoopFunctions(UInt32 un_pixels_per_meter) :
118  m_cLoopFunctions(CSimulator::GetInstance().GetLoopFunctions()),
119  m_unPixelsPerMeter(un_pixels_per_meter) {
120  const CVector3& cArenaSize = CSimulator::GetInstance().GetSpace().GetArenaSize();
121  m_cHalfArenaSize.Set(
122  cArenaSize.GetX() * 0.5f,
123  cArenaSize.GetY() * 0.5f);
124  const CVector3& cArenaCenter = CSimulator::GetInstance().GetSpace().GetArenaCenter();
125  m_cArenaCenter.Set(cArenaCenter.GetX(),
126  cArenaCenter.GetY());
127  }
128 
130  Real f_y) {
131  return m_cLoopFunctions.GetFloorColor(CVector2(f_x, f_y));
132  }
133 
134 #ifdef ARGOS_WITH_FREEIMAGE
135  virtual void SaveAsImage(const std::string& str_path) {
136  fipImage cImage(FIT_BITMAP, m_unPixelsPerMeter * m_cHalfArenaSize.GetX()*2, m_unPixelsPerMeter * m_cHalfArenaSize.GetY()*2, 24);
137  Real fFactor = 1.0f / static_cast<Real>(m_unPixelsPerMeter);
138  CVector2 cFloorPos;
139  CColor cARGoSPixel;
140  RGBQUAD tFIPPixel;
141  for(UInt32 y = 0; y < cImage.getHeight(); ++y) {
142  for(UInt32 x = 0; x < cImage.getWidth(); ++x) {
143  cFloorPos.Set(x * fFactor, y * fFactor);
144  cFloorPos -= m_cHalfArenaSize;
145  cFloorPos += m_cArenaCenter;
146  cARGoSPixel = m_cLoopFunctions.GetFloorColor(cFloorPos);
147  tFIPPixel.rgbRed = cARGoSPixel.GetRed();
148  tFIPPixel.rgbGreen = cARGoSPixel.GetGreen();
149  tFIPPixel.rgbBlue = cARGoSPixel.GetBlue();
150  cImage.setPixelColor(x, y, &tFIPPixel);
151  }
152  }
153  if(!cImage.save(str_path.c_str())) {
154  THROW_ARGOSEXCEPTION("Cannot save image \"" << str_path << "\" for floor entity.");
155  }
156  }
157 #endif
158 
159  private:
160 
161  CLoopFunctions& m_cLoopFunctions;
162  UInt32 m_unPixelsPerMeter;
163  CVector2 m_cHalfArenaSize;
164  CVector2 m_cArenaCenter;
165  };
166 
167  /****************************************/
168  /****************************************/
169 
171  CEntity(NULL),
172  m_eColorSource(UNSET),
173  m_pcColorSource(NULL),
174  m_bHasChanged(true) {}
175 
176  /****************************************/
177  /****************************************/
178 
179 #ifdef ARGOS_WITH_FREEIMAGE
180  CFloorEntity::CFloorEntity(const std::string& str_id,
181  const std::string& str_file_name) :
182  CEntity(NULL, str_id),
183  m_eColorSource(FROM_IMAGE),
184  m_pcColorSource(NULL),
185  m_bHasChanged(true) {
186  std::string strFileName = str_file_name;
187  ExpandEnvVariables(strFileName);
188  m_pcColorSource = new CFloorColorFromImageFile(strFileName);
189  }
190 #endif
191 
192  /****************************************/
193  /****************************************/
194 
195  CFloorEntity::CFloorEntity(const std::string& str_id,
196  UInt32 un_pixels_per_meter) :
197  CEntity(NULL, str_id),
198  m_eColorSource(FROM_LOOP_FUNCTIONS),
199  m_pcColorSource(new CFloorColorFromLoopFunctions(un_pixels_per_meter)),
200  m_bHasChanged(true) {}
201 
202  /****************************************/
203  /****************************************/
204 
206  if(m_pcColorSource != NULL) {
207  delete m_pcColorSource;
208  }
209  }
210 
211  /****************************************/
212  /****************************************/
213 
215  /* Init parent */
216  CEntity::Init(t_tree);
217  /* Parse XML */
218  std::string strColorSource;
219  GetNodeAttribute(t_tree, "source", strColorSource);
220  if(strColorSource == "loop_functions") {
221  m_eColorSource = FROM_LOOP_FUNCTIONS;
222  UInt32 unPixelsPerMeter;
223  GetNodeAttribute(t_tree, "pixels_per_meter", unPixelsPerMeter);
224  m_pcColorSource = new CFloorColorFromLoopFunctions(unPixelsPerMeter);
225  }
226  else if(strColorSource == "image") {
227 #ifdef ARGOS_WITH_FREEIMAGE
228  m_eColorSource = FROM_IMAGE;
229  std::string strPath;
230  GetNodeAttribute(t_tree, "path", strPath);
231  ExpandEnvVariables(strPath);
232  m_pcColorSource = new CFloorColorFromImageFile(strPath);
233 #else
234  THROW_ARGOSEXCEPTION("ARGoS was compiled without FreeImage, this image source is unsupported for the floor entity \"" <<
235  GetId() <<
236  "\"");
237 #endif
238  }
239  else {
240  THROW_ARGOSEXCEPTION("Unknown image source \"" <<
241  strColorSource <<
242  "\" for the floor entity \"" <<
243  GetId() <<
244  "\"");
245  }
246  }
247 
248  /****************************************/
249  /****************************************/
250 
252  m_pcColorSource->Reset();
253  }
254 
255  /****************************************/
256  /****************************************/
257 
258 #ifdef ARGOS_WITH_FREEIMAGE
259  void CFloorEntity::SaveAsImage(const std::string& str_path) {
260  m_pcColorSource->SaveAsImage(str_path);
261  }
262 #endif
263 
264  /****************************************/
265  /****************************************/
266 
267  REGISTER_ENTITY(CFloorEntity,
268  "floor",
269  "Carlo Pinciroli [ilpincy@gmail.com]",
270  "1.0",
271  "It contains the properties of the arena floor.",
272  "The floor entity contains the properties of the arena floor. In the current\n"
273  "implementation, it contains only the color of the floor. The floor color is\n"
274  "detected by the robots' ground sensors.\n\n"
275  "REQUIRED XML CONFIGURATION\n\n"
276  " <arena ...>\n"
277  " ...\n"
278  " <floor id=\"floor\"\n"
279  " source=\"SOURCE\" />\n"
280  " ...\n"
281  " </arena>\n\n"
282  "The 'id' attribute is necessary and must be unique among the entities. If two\n"
283  "entities share the same id, initialization aborts.\n"
284  "The 'source' attribute specifies where to get the color of the floor from. Its\n"
285  "value, here denoted as SOURCE, can assume the following values:\n\n"
286  " image The color is calculated from the passed image file\n"
287  " loop_functions The color is calculated calling the loop functions\n\n"
288  "When 'source' is set to 'image', as showed in the following example, you have\n"
289  "to specify the image path in the additional attribute 'path':\n\n"
290  " <arena ...>\n"
291  " ...\n"
292  " <floor id=\"floor\"\n"
293  " source=\"image\"\n"
294  " path=\"/path/to/imagefile.ext\" />\n"
295  " ...\n"
296  " </arena>\n\n"
297  "Many image formats are available, such as PNG, JPG, BMP, GIF and many more.\n"
298  "Refer to the FreeImage webpage for a complete list of supported image formats\n"
299  "(http://freeimage.sourceforge.net/features.html).\n\n"
300  "When 'source' is set to 'loop_functions', as showed in the following example,\n"
301  "an image is implicitly created to be used as texture for graphical\n"
302  "visualizations. The algorithm that creates the texture needs to convert from\n"
303  "meters (in the arena) to pixels (of the texture). You control how many pixels\n"
304  "per meter are used with the attribute 'pixels_per_meter'. Clearly, the higher\n"
305  "value, the higher the quality, but also the slower the algorithm and the bigger\n"
306  "the texture. The algorithm is called only once at init time, so the fact that\n"
307  "it is slow is not so important. However, the image size is limited by OpenGL.\n"
308  "Every implementation has its own limit, and you should check yours if any\n"
309  "texture-related problem arises. Now for the example:\n\n"
310  " <arena ...>\n"
311  " ...\n"
312  " <floor id=\"floor\"\n"
313  " source=\"loop_functions\"\n"
314  " pixels_per_meter=\"100\" />\n"
315  " ...\n"
316  " </arena>\n\n"
317  "OPTIONAL XML CONFIGURATION\n\n"
318  "None for the time being.\n",
319  "Usable"
320  );
321 
322  /****************************************/
323  /****************************************/
324 
326  public:
327  void ApplyTo(CSpace& c_space, CFloorEntity& c_entity) {
328  c_space.AddEntity(c_entity);
329  c_space.SetFloorEntity(c_entity);
330  }
331  };
332 
333  REGISTER_SPACE_OPERATION(CSpaceOperationAddEntity,
334  CSpaceOperationAddCFloorEntity,
335  CFloorEntity);
337 
338  /****************************************/
339  /****************************************/
340 
341 }
const CVector3 & GetArenaCenter() const
Returns the arena center.
Definition: space.h:368
void AddEntity(ENTITY &c_entity)
Adds an entity of the given type.
Definition: space.h:253
A 3D vector class.
Definition: vector3.h:29
std::string & ExpandEnvVariables(std::string &str_buffer)
Searches into str_buffer for occurrences of an environment variable of the form $VAR and substitutes ...
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector2.h:78
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
The basic entity type.
Definition: entity.h:89
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
virtual void Reset()
Restores the initial state of the floor.
UInt8 GetGreen() const
Returns the green channel of the color.
Definition: color.h:90
CFloorEntity()
Class constructor.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector2.h:94
CFloorColorFromLoopFunctions(UInt32 un_pixels_per_meter)
REGISTER_ENTITY(CFloorEntity,"floor","Carlo Pinciroli [ilpincy@gmail.com]","1.0","It contains the properties of the arena floor.","The floor entity contains the properties of the arena floor. In the current\n""implementation, it contains only the color of the floor. The floor color is\n""detected by the robots' ground sensors.\n\n""REQUIRED XML CONFIGURATION\n\n"" <arena ...>\n"" ...\n"" <floor id=\"floor\"\n"" source=\"SOURCE\" />\n"" ...\n"" </arena>\n\n""The 'id' attribute is necessary and must be unique among the entities. If two\n""entities share the same id, initialization aborts.\n""The 'source' attribute specifies where to get the color of the floor from. Its\n""value, here denoted as SOURCE, can assume the following values:\n\n"" image The color is calculated from the passed image file\n"" loop_functions The color is calculated calling the loop functions\n\n""When 'source' is set to 'image', as showed in the following example, you have\n""to specify the image path in the additional attribute 'path':\n\n"" <arena ...>\n"" ...\n"" <floor id=\"floor\"\n"" source=\"image\"\n"" path=\"/path/to/imagefile.ext\" />\n"" ...\n"" </arena>\n\n""Many image formats are available, such as PNG, JPG, BMP, GIF and many more.\n""Refer to the FreeImage webpage for a complete list of supported image formats\n""(http://freeimage.sourceforge.net/features.html).\n\n""When 'source' is set to 'loop_functions', as showed in the following example,\n""an image is implicitly created to be used as texture for graphical\n""visualizations. The algorithm that creates the texture needs to convert from\n""meters (in the arena) to pixels (of the texture). You control how many pixels\n""per meter are used with the attribute 'pixels_per_meter'. Clearly, the higher\n""value, the higher the quality, but also the slower the algorithm and the bigger\n""the texture. The algorithm is called only once at init time, so the fact that\n""it is slow is not so important. However, the image size is limited by OpenGL.\n""Every implementation has its own limit, and you should check yours if any\n""texture-related problem arises. Now for the example:\n\n"" <arena ...>\n"" ...\n"" <floor id=\"floor\"\n"" source=\"loop_functions\"\n"" pixels_per_meter=\"100\" />\n"" ...\n"" </arena>\n\n""OPTIONAL XML CONFIGURATION\n\n""None for the time being.\n","Usable")
REGISTER_SPACE_OPERATION(CSpaceOperationAddEntity, CSpaceOperationAddCFloorEntity, CFloorEntity)
const CVector3 & GetArenaSize() const
Returns the arena size.
Definition: space.h:350
unsigned int UInt32
32-bit unsigned integer.
Definition: datatypes.h:97
void Set(const Real f_x, const Real f_y, const Real f_z)
Sets the vector contents from Cartesian coordinates.
Definition: vector3.h:143
REGISTER_STANDARD_SPACE_OPERATION_REMOVE_ENTITY(CFloorEntity)
A 2D vector class.
Definition: vector2.h:25
void ApplyTo(CSpace &c_space, CFloorEntity &c_entity)
virtual void Init(TConfigurationNode &t_tree)
Initializes the entity from an XML tree.
UInt8 GetRed() const
Returns the red channel of the color.
Definition: color.h:79
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
virtual ~CFloorEntity()
Class destructor.
virtual void Init(TConfigurationNode &t_tree)
Initializes the state of the entity from the XML configuration tree.
Definition: entity.cpp:40
The basic color type.
Definition: color.h:25
void SetFloorEntity(CFloorEntity &c_floor_entity)
Sets the floor entity.
Definition: space.h:227
UInt8 GetBlue() const
Returns the blue channel of the color.
Definition: color.h:101
void Set(Real f_x, Real f_y)
Sets the vector contents from Cartesian coordinates.
Definition: vector2.h:111
const std::string & GetId() const
Returns the id of this entity.
Definition: entity.h:157
virtual CColor GetColorAtPoint(Real f_x, Real f_y)
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
The core class of ARGOS.
Definition: simulator.h:62
virtual CColor GetFloorColor(const CVector2 &c_pos_on_floor)
Returns the color of the floor in the specified point.
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:78
CSpace & GetSpace() const
Returns a reference to the simulated space.
Definition: simulator.h:104