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 "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 }