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