ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "range_and_bearing_medium_sensor.h" 00008 #include <argos3/core/simulator/simulator.h> 00009 #include <argos3/core/simulator/entity/composable_entity.h> 00010 #include <argos3/core/simulator/entity/controllable_entity.h> 00011 #include <argos3/plugins/simulator/entities/rab_equipped_entity.h> 00012 #include <argos3/plugins/simulator/media/rab_medium.h> 00013 00014 namespace argos { 00015 00016 /****************************************/ 00017 /****************************************/ 00018 00019 CRange<CRadians> INCLINATION_RANGE(CRadians(0), CRadians(ARGOS_PI)); 00020 00021 /****************************************/ 00022 /****************************************/ 00023 00024 CRangeAndBearingMediumSensor::CRangeAndBearingMediumSensor() : 00025 m_pcRangeAndBearingEquippedEntity(NULL), 00026 m_fDistanceNoiseStdDev(0.0f), 00027 m_pcRNG(NULL), 00028 m_cSpace(CSimulator::GetInstance().GetSpace()), 00029 m_bShowRays(false) {} 00030 00031 /****************************************/ 00032 /****************************************/ 00033 00034 void CRangeAndBearingMediumSensor::SetRobot(CComposableEntity& c_entity) { 00035 /* Assign RAB equipped entity to this sensor */ 00036 m_pcRangeAndBearingEquippedEntity = &c_entity.GetComponent<CRABEquippedEntity>("rab"); 00037 /* Enable the RAB equipped entity */ 00038 m_pcRangeAndBearingEquippedEntity->SetCanBeEnabledIfDisabled(true); 00039 m_pcRangeAndBearingEquippedEntity->Enable(); 00040 /* Get reference to controllable entity */ 00041 m_pcControllableEntity = &c_entity.GetComponent<CControllableEntity>("controller"); 00042 } 00043 00044 /****************************************/ 00045 /****************************************/ 00046 00047 void CRangeAndBearingMediumSensor::Init(TConfigurationNode& t_tree) { 00048 try { 00049 /* Parent class init */ 00050 CCI_RangeAndBearingSensor::Init(t_tree); 00051 /* Show rays? */ 00052 GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays); 00053 /* Parse noise */ 00054 GetNodeAttributeOrDefault(t_tree, "noise_std_dev", m_fDistanceNoiseStdDev, m_fDistanceNoiseStdDev); 00055 if(m_fDistanceNoiseStdDev > 0.0f) { 00056 m_pcRNG = CRandom::CreateRNG("argos"); 00057 } 00058 /* Get RAB medium from id specified in the XML */ 00059 std::string strMedium; 00060 GetNodeAttribute(t_tree, "medium", strMedium); 00061 m_pcRangeAndBearingMedium = &(CSimulator::GetInstance().GetMedium<CRABMedium>(strMedium)); 00062 /* Assign RAB entity to the medium */ 00063 m_pcRangeAndBearingMedium->AddEntity(*m_pcRangeAndBearingEquippedEntity); 00064 } 00065 catch(CARGoSException& ex) { 00066 THROW_ARGOSEXCEPTION_NESTED("Error initializing the range and bearing medium sensor", ex); 00067 } 00068 } 00069 00070 /****************************************/ 00071 /****************************************/ 00072 00073 void CRangeAndBearingMediumSensor::Update() { 00075 /* Delete old readings */ 00076 m_tReadings.clear(); 00077 /* Get list of communicating RABs */ 00078 const CSet<CRABEquippedEntity*>& setRABs = m_pcRangeAndBearingMedium->GetRABsCommunicatingWith(*m_pcRangeAndBearingEquippedEntity); 00079 /* Buffer for calculating the message--robot distance */ 00080 CVector3 cVectorRobotToMessage; 00081 /* Buffer for the received packet */ 00082 CCI_RangeAndBearingSensor::SPacket sPacket; 00083 /* Go through communicating RABs and create packets */ 00084 for(CSet<CRABEquippedEntity*>::iterator it = setRABs.begin(); 00085 it != setRABs.end(); ++it) { 00086 /* Create a reference to the RAB entity to process */ 00087 CRABEquippedEntity& cRABEntity = **it; 00088 /* Add ray if requested */ 00089 if(m_bShowRays) { 00090 m_pcControllableEntity->AddCheckedRay(false, 00091 CRay3(cRABEntity.GetPosition(), 00092 m_pcRangeAndBearingEquippedEntity->GetPosition())); 00093 } 00094 /* Calculate vector to entity */ 00095 cVectorRobotToMessage = cRABEntity.GetPosition(); 00096 cVectorRobotToMessage -= m_pcRangeAndBearingEquippedEntity->GetPosition(); 00097 /* If noise was setup, add it */ 00098 if(m_fDistanceNoiseStdDev > 0.0f) { 00099 cVectorRobotToMessage += CVector3( 00100 m_pcRNG->Gaussian(m_fDistanceNoiseStdDev), 00101 m_pcRNG->Uniform(INCLINATION_RANGE), 00102 m_pcRNG->Uniform(CRadians::UNSIGNED_RANGE)); 00103 } 00104 /* 00105 * Set range and bearing from cVectorRobotToMessage 00106 * First, we must rotate the cVectorRobotToMessage so that 00107 * it is local to the robot coordinate system. To do this, 00108 * it enough to rotate cVectorRobotToMessage by the inverse 00109 * of the robot orientation. 00110 */ 00111 cVectorRobotToMessage.Rotate(m_pcRangeAndBearingEquippedEntity->GetOrientation().Inverse()); 00112 cVectorRobotToMessage.ToSphericalCoords(sPacket.Range, 00113 sPacket.VerticalBearing, 00114 sPacket.HorizontalBearing); 00115 /* Convert range to cm */ 00116 sPacket.Range *= 100.0f; 00117 /* Normalize horizontal bearing between [-pi,pi] */ 00118 sPacket.HorizontalBearing.SignedNormalize(); 00119 /* 00120 * The vertical bearing is defined as the angle between the local 00121 * robot XY plane and the message source position, i.e., the elevation 00122 * in math jargon. However, cVectorRobotToMessage.ToSphericalCoords() 00123 * sets sPacket.VerticalBearing to the inclination, which is the angle 00124 * between the azimuth vector (robot local Z axis) and 00125 * cVectorRobotToMessage. Elevation = 90 degrees - Inclination. 00126 */ 00127 sPacket.VerticalBearing.Negate(); 00128 sPacket.VerticalBearing += CRadians::PI_OVER_TWO; 00129 sPacket.VerticalBearing.SignedNormalize(); 00130 /* Set message data */ 00131 sPacket.Data = cRABEntity.GetData(); 00132 /* Add message to the list */ 00133 m_tReadings.push_back(sPacket); 00134 } 00135 } 00136 00137 /****************************************/ 00138 /****************************************/ 00139 00140 void CRangeAndBearingMediumSensor::Reset() { 00141 m_tReadings.clear(); 00142 } 00143 00144 /****************************************/ 00145 /****************************************/ 00146 00147 void CRangeAndBearingMediumSensor::Destroy() { 00148 m_pcRangeAndBearingMedium->RemoveEntity(*m_pcRangeAndBearingEquippedEntity); 00149 } 00150 00151 /****************************************/ 00152 /****************************************/ 00153 00154 REGISTER_SENSOR(CRangeAndBearingMediumSensor, 00155 "range_and_bearing", "medium", 00156 "Carlo Pinciroli [ilpincy@gmail.com]", 00157 "1.0", 00158 "The range-and-bearing sensor.", 00159 "This sensor allows robots to perform situated communication, i.e., a form of\n" 00160 "wireless communication whereby the receiver also knows the location of the\n" 00161 "sender with respect to its own frame of reference.\n" 00162 "This implementation of the range-and-bearing sensor is associated to the\n" 00163 "range-and-bearing medium. To be able to use this sensor, you must add a\n" 00164 "range-and-bearing medium to the <media> section.\n" 00165 "This sensor allows a robot to receive messages. To send messages, you need the\n" 00166 "range-and-bearing actuator.\n" 00167 "To use this sensor, in controllers you must include the\n" 00168 "ci_range_and_bearing_sensor.h header.\n\n" 00169 "REQUIRED XML CONFIGURATION\n\n" 00170 " <controllers>\n" 00171 " ...\n" 00172 " <my_controller ...>\n" 00173 " ...\n" 00174 " <sensors>\n" 00175 " ...\n" 00176 " <range_and_bearing implementation=\"medium\"\n" 00177 " medium=\"rab\" />\n" 00178 " ...\n" 00179 " </sensors>\n" 00180 " ...\n" 00181 " </my_controller>\n" 00182 " ...\n" 00183 " </controllers>\n\n" 00184 "The 'medium' attribute must be set to the id of the range-and-bearing medium\n" 00185 "declared in the <media> section.\n\n" 00186 "OPTIONAL XML CONFIGURATION\n\n" 00187 "It is possible to draw the rays shot by the range-and-bearing sensor in the\n" 00188 "OpenGL visualization. This can be useful for sensor debugging but also to\n" 00189 "understand what's wrong in your controller. In OpenGL, the rays are drawn in\n" 00190 "cyan when two robots are communicating.\n" 00191 "To turn this functionality on, add the attribute \"show_rays\" as in this\n" 00192 "example:\n\n" 00193 " <controllers>\n" 00194 " ...\n" 00195 " <my_controller ...>\n" 00196 " ...\n" 00197 " <sensors>\n" 00198 " ...\n" 00199 " <range_and_bearing implementation=\"medium\"\n" 00200 " medium=\"rab\"\n" 00201 " show_rays=\"true\" />\n" 00202 " ...\n" 00203 " </sensors>\n" 00204 " ...\n" 00205 " </my_controller>\n" 00206 " ...\n" 00207 " </controllers>\n\n" 00208 "It is possible to add noise to the readings, thus matching the characteristics\n" 00209 "of a real robot better. Noise is implemented as a random vector added to the\n" 00210 "vector joining two communicating robots. For the random vector, the inclination\n" 00211 "and azimuth are chosen uniformly in the range [0:PI] and [0:2PI], respectively,\n" 00212 "and the length is drawn from a Gaussian distribution. The standard deviation of\n" 00213 "the Gaussian distribution is expressed in meters and set by the user through\n" 00214 "the attribute 'noise_std_dev' as shown in this example:\n\n" 00215 " <controllers>\n" 00216 " ...\n" 00217 " <my_controller ...>\n" 00218 " ...\n" 00219 " <sensors>\n" 00220 " ...\n" 00221 " <range_and_bearing implementation=\"medium\"\n" 00222 " medium=\"rab\"\n" 00223 " noise_std_dev=\"0.1\" />\n" 00224 " ...\n" 00225 " </sensors>\n" 00226 " ...\n" 00227 " </my_controller>\n" 00228 " ...\n" 00229 " </controllers>\n", 00230 "Usable"); 00231 00232 }