ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/sensors/range_and_bearing_medium_sensor.cpp
Go to the documentation of this file.
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 }