ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/robots/foot-bot/simulator/footbot_motor_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 "footbot_motor_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    CFootBotMotorGroundRotZOnlySensor::CFootBotMotorGroundRotZOnlySensor() :
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 CFootBotMotorGroundRotZOnlySensor::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 CFootBotMotorGroundRotZOnlySensor::Init(TConfigurationNode& t_tree) {
00048       try {
00049          CCI_FootBotMotorGroundSensor::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 foot-bot 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(4);
00062       }
00063       catch(CARGoSException& ex) {
00064          THROW_ARGOSEXCEPTION_NESTED("Initialization error in foot-bot rotzonly ground sensor", ex);
00065       }
00066    }
00067 
00068    /****************************************/
00069    /****************************************/
00070 
00071    void CFootBotMotorGroundRotZOnlySensor::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].Value = cColor.ToGrayScale() / 255.0f;
00095          /* Apply noise to the sensor */
00096          if(m_bAddNoise) {
00097             m_tReadings[i].Value += m_pcRNG->Uniform(m_cNoiseRange);
00098          }
00099          /* Clamp the reading between 0 and 1 */
00100          UNIT.TruncValue(m_tReadings[i].Value);
00101       }
00102    }
00103 
00104    /****************************************/
00105    /****************************************/
00106 
00107    void CFootBotMotorGroundRotZOnlySensor::Reset() {
00108       for(UInt32 i = 0; i < GetReadings().size(); ++i) {
00109          m_tReadings[i].Value = 0.0f;
00110       }
00111    }
00112 
00113    /****************************************/
00114    /****************************************/
00115 
00116    REGISTER_SENSOR(CFootBotMotorGroundRotZOnlySensor,
00117                    "footbot_motor_ground", "rot_z_only",
00118                    "Carlo Pinciroli [ilpincy@gmail.com]",
00119                    "1.0",
00120                    "The foot-bot motor ground sensor.",
00121                    "This sensor accesses the foot-bot motor ground sensor. For a complete description\n"
00122                    "of its usage, refer to the ci_footbot_motor_ground_sensor.h interface. For the XML\n"
00123                    "configuration, refer to the default ground sensor.\n",
00124                    "Usable"
00125                   );
00126 
00127 }