ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/sensors/ground_rotzonly_sensor.cpp
Go to the documentation of this file.
00001 
00007 #include <argos3/core/simulator/simulator.h>
00008 #include <argos3/core/simulator/entity/composable_entity.h>
00009 #include <argos3/core/simulator/entity/embodied_entity.h>
00010 #include <argos3/core/simulator/entity/floor_entity.h>
00011 #include <argos3/plugins/simulator/entities/ground_sensor_equipped_entity.h>
00012 
00013 #include "ground_rotzonly_sensor.h"
00014 
00015 namespace argos {
00016 
00017    /****************************************/
00018    /****************************************/
00019 
00020    static CRange<Real> UNIT(0.0f, 1.0f);
00021 
00022    /****************************************/
00023    /****************************************/
00024 
00025    CGroundRotZOnlySensor::CGroundRotZOnlySensor() :
00026       m_pcEmbodiedEntity(NULL),
00027       m_pcFloorEntity(NULL),
00028       m_pcGroundSensorEntity(NULL),
00029       m_pcRNG(NULL),
00030       m_bAddNoise(false),
00031       m_cSpace(CSimulator::GetInstance().GetSpace()) {}
00032 
00033    /****************************************/
00034    /****************************************/
00035 
00036    void CGroundRotZOnlySensor::SetRobot(CComposableEntity& c_entity) {
00037       m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body"));
00038       m_pcGroundSensorEntity = &(c_entity.GetComponent<CGroundSensorEquippedEntity>("ground_sensors"));
00039       m_pcGroundSensorEntity->SetCanBeEnabledIfDisabled(true);
00040       m_pcGroundSensorEntity->Enable();
00041       m_pcFloorEntity = &m_cSpace.GetFloorEntity();
00042    }
00043 
00044    /****************************************/
00045    /****************************************/
00046 
00047    void CGroundRotZOnlySensor::Init(TConfigurationNode& t_tree) {
00048       try {
00049          CCI_GroundSensor::Init(t_tree);
00050          /* Parse noise level */
00051          Real fNoiseLevel = 0.0f;
00052          GetNodeAttributeOrDefault(t_tree, "noise_level", fNoiseLevel, fNoiseLevel);
00053          if(fNoiseLevel < 0.0f) {
00054             THROW_ARGOSEXCEPTION("Can't specify a negative value for the noise level of the ground sensor");
00055          }
00056          else if(fNoiseLevel > 0.0f) {
00057             m_bAddNoise = true;
00058             m_cNoiseRange.Set(-fNoiseLevel, fNoiseLevel);
00059             m_pcRNG = CRandom::CreateRNG("argos");
00060          }
00061          m_tReadings.resize(m_pcGroundSensorEntity->GetNumSensors());
00062       }
00063       catch(CARGoSException& ex) {
00064          THROW_ARGOSEXCEPTION_NESTED("Initialization error in rotzonly ground sensor", ex);
00065       }
00066    }
00067 
00068    /****************************************/
00069    /****************************************/
00070    
00071    void CGroundRotZOnlySensor::Update() {
00072       /*
00073        * We make the assumption that the robot is rotated only wrt to Z
00074        */
00075       /* Get robot position and orientation */
00076       const CVector3& cEntityPos = m_pcEmbodiedEntity->GetPosition();
00077       const CQuaternion& cEntityRot = m_pcEmbodiedEntity->GetOrientation();
00078       CRadians cRotZ, cRotY, cRotX;
00079       cEntityRot.ToEulerAngles(cRotZ, cRotY, cRotX);
00080       /* Set robot center */
00081       CVector2 cCenterPos(cEntityPos.GetX(), cEntityPos.GetY());
00082       /* Position of sensor on the ground after rototranslation */
00083       CVector2 cSensorPos;
00084       /* Go through the sensors */
00085       for(UInt32 i = 0; i < m_tReadings.size(); ++i) {
00086          /* Calculate sensor position on the ground */
00087          cSensorPos = m_pcGroundSensorEntity->GetSensor(i).Offset;
00088          cSensorPos.Rotate(cRotZ);
00089          cSensorPos += cCenterPos;
00090          /* Get the color */
00091          const CColor& cColor = m_pcFloorEntity->GetColorAtPoint(cSensorPos.GetX(),
00092                                                                  cSensorPos.GetY());
00093          /* Set the reading */
00094          m_tReadings[i] = cColor.ToGrayScale() / 255.0f;
00095          /* Apply noise to the sensor */
00096          if(m_bAddNoise) {
00097             m_tReadings[i] += m_pcRNG->Uniform(m_cNoiseRange);
00098          }
00099          /* Is it a BW sensor? */
00100          if(m_pcGroundSensorEntity->GetSensor(i).Type == CGroundSensorEquippedEntity::TYPE_BLACK_WHITE) {
00101             /* Yes, set 0 or 1 */
00102             m_tReadings[i] = m_tReadings[i] < 0.5f ? 0.0f : 1.0f;
00103          }
00104          else {
00105             /* No, clamp the reading between 0 and 1 */
00106             UNIT.TruncValue(m_tReadings[i]);
00107          }
00108       }
00109    }
00110 
00111    /****************************************/
00112    /****************************************/
00113 
00114    void CGroundRotZOnlySensor::Reset() {
00115       for(UInt32 i = 0; i < GetReadings().size(); ++i) {
00116          m_tReadings[i] = 0.0f;
00117       }
00118    }
00119 
00120    /****************************************/
00121    /****************************************/
00122 
00123    REGISTER_SENSOR(CGroundRotZOnlySensor,
00124                    "ground", "rot_z_only",
00125                    "Carlo Pinciroli [ilpincy@gmail.com]",
00126                    "1.0",
00127                    "A generic ground sensor (optimized for 2D).",
00128                    "This sensor accesses a set of ground sensors. The sensors all return a value\n"
00129                    "between 0 and 1, where 0 means black and 1 means white. Depending on the type\n"
00130                    "of ground sensor, readings can either take 0 or 1 as value (bw sensors) or a\n"
00131                    "value in between (grayscale sensors). In controllers, you must include the\n"
00132                    "ci_ground_sensor.h header.\n\n"
00133                    "REQUIRED XML CONFIGURATION\n\n"
00134                    "  <controllers>\n"
00135                    "    ...\n"
00136                    "    <my_controller ...>\n"
00137                    "      ...\n"
00138                    "      <sensors>\n"
00139                    "        ...\n"
00140                    "        <ground implementation=\"rot_z_only\" />\n"
00141                    "        ...\n"
00142                    "      </sensors>\n"
00143                    "      ...\n"
00144                    "    </my_controller>\n"
00145                    "    ...\n"
00146                    "  </controllers>\n\n"
00147                    "OPTIONAL XML CONFIGURATION\n\n"
00148                    "It is possible to add uniform noise to the sensors, thus matching the\n"
00149                    "characteristics of a real robot better. This can be done with the attribute\n"
00150                    "\"noise_level\", whose allowed range is in [-1,1] and is added to the calculated\n"
00151                    "reading. The final sensor reading is always normalized in the [0-1] range.\n\n"
00152                    "  <controllers>\n"
00153                    "    ...\n"
00154                    "    <my_controller ...>\n"
00155                    "      ...\n"
00156                    "      <sensors>\n"
00157                    "        ...\n"
00158                    "        <ground implementation=\"rot_z_only\"\n"
00159                    "                noise_level=\"0.1\" />\n"
00160                    "        ...\n"
00161                    "      </sensors>\n"
00162                    "      ...\n"
00163                    "    </my_controller>\n"
00164                    "    ...\n"
00165                    "  </controllers>\n\n"
00166                    "OPTIONAL XML CONFIGURATION\n\n"
00167                    "None.\n",
00168                    "Usable"
00169                   );
00170 
00171 }