ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/robots/foot-bot/simulator/footbot_distance_scanner_rotzonly_sensor.cpp
Go to the documentation of this file.
00001 
00007 #include "footbot_distance_scanner_rotzonly_sensor.h"
00008 #include <argos3/core/simulator/entity/composable_entity.h>
00009 #include <argos3/core/simulator/entity/controllable_entity.h>
00010 #include <argos3/core/simulator/simulator.h>
00011 #include <argos3/core/simulator/space/space.h>
00012 
00013 namespace argos {
00014 
00015    /****************************************/
00016    /****************************************/
00017 
00018    static const Real FOOTBOT_RADIUS           = 0.085036758f;
00019 
00020    static const Real SHORT_RANGE_MIN_DISTANCE = 0.0f;
00021    static const Real SHORT_RANGE_RAY_START    = FOOTBOT_RADIUS;
00022    static const Real SHORT_RANGE_RAY_END      = FOOTBOT_RADIUS + 0.26f;
00023 
00024    static const Real LONG_RANGE_MIN_DISTANCE  = 0.12f;
00025    static const Real LONG_RANGE_RAY_START     = FOOTBOT_RADIUS;
00026    static const Real LONG_RANGE_RAY_END       = FOOTBOT_RADIUS + 1.42f;
00027 
00028    static const Real SENSOR_ELEVATION         = 0.123199866f;
00029 
00030    /****************************************/
00031    /****************************************/
00032 
00033    CFootBotDistanceScannerRotZOnlySensor::CFootBotDistanceScannerRotZOnlySensor() :
00034       m_pcRNG(NULL),
00035       m_bAddNoise(false),
00036       m_cSpace(CSimulator::GetInstance().GetSpace()),
00037       m_bShowRays(false) {}
00038 
00039    /****************************************/
00040    /****************************************/
00041 
00042    void CFootBotDistanceScannerRotZOnlySensor::Init(TConfigurationNode& t_tree) {
00043       try {
00044          CCI_FootBotDistanceScannerSensor::Init(t_tree);
00045          /* Show rays? */
00046          GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
00047          /* Noise range */
00048          GetNodeAttributeOrDefault(t_tree, "noise_range", m_cNoiseRange, m_cNoiseRange);
00049          if(m_cNoiseRange.GetSpan() > 0.0f) {
00050             m_bAddNoise = true;
00051             m_pcRNG = CRandom::CreateRNG("argos");
00052          }
00053       }
00054       catch(CARGoSException& ex) {
00055          THROW_ARGOSEXCEPTION_NESTED("Initialization error in foot-bot distance scanner rot_z_only sensor.", ex);
00056       }
00057    }
00058 
00059    /****************************************/
00060    /****************************************/
00061 
00062    void CFootBotDistanceScannerRotZOnlySensor::SetRobot(CComposableEntity& c_entity) {
00063       m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body"));
00064       m_pcControllableEntity = &(c_entity.GetComponent<CControllableEntity>("controller"));
00065       m_pcDistScanEntity = &(c_entity.GetComponent<CFootBotDistanceScannerEquippedEntity>("distance_scanner"));
00066       m_pcDistScanEntity->SetCanBeEnabledIfDisabled(true);
00067       m_pcDistScanEntity->Enable();
00068    }
00069 
00070    /****************************************/
00071    /****************************************/
00072 
00073    void CFootBotDistanceScannerRotZOnlySensor::Update() {
00074       /* Clear the maps */
00075       m_tReadingsMap.clear();
00076       m_tShortReadingsMap.clear();
00077       m_tLongReadingsMap.clear();
00078       /* Perform calculations only if the sensor is on */
00079       if(m_pcDistScanEntity->GetMode() != CFootBotDistanceScannerEquippedEntity::MODE_OFF) {
00080          /* Update the readings wrt to device mode */
00081          if(m_pcDistScanEntity->GetMode() == CFootBotDistanceScannerEquippedEntity::MODE_POSITION_CONTROL) {
00082             /* Sensor blocked in a position */
00083             /* Recalculate the rays */
00084             CalculateRaysNotRotating();
00085             /* Save the rotation for next time */
00086             m_cLastDistScanRotation = m_pcDistScanEntity->GetRotation();
00087             /* Update the values */
00088             UpdateNotRotating();
00089          }
00090          else {
00091             /* Rotating sensor */
00092             /* Recalculate the rays */
00093             CalculateRaysRotating();
00094             /* Update the values */
00095             UpdateRotating();
00096             /* Save the rotation for next time */
00097             m_cLastDistScanRotation = m_pcDistScanEntity->GetRotation();
00098          }
00099       }
00100    }
00101 
00102    /****************************************/
00103    /****************************************/
00104 
00105    void CFootBotDistanceScannerRotZOnlySensor::Reset() {
00106       /* Clear the maps */
00107       m_tReadingsMap.clear();
00108       m_tShortReadingsMap.clear();
00109       m_tLongReadingsMap.clear();
00110       /* Zero the last rotation */
00111       m_cLastDistScanRotation = CRadians::ZERO;
00112    }
00113 
00114    /****************************************/
00115    /****************************************/
00116 
00117    void CFootBotDistanceScannerRotZOnlySensor::UpdateNotRotating() {
00118       /* Short range [0] */
00119       CRadians cAngle = m_cLastDistScanRotation;
00120       cAngle.SignedNormalize();
00121       Real fReading = CalculateReadingForRay(m_cShortRangeRays0[0], SHORT_RANGE_MIN_DISTANCE);
00122       m_tShortReadingsMap[cAngle] = fReading;
00123       m_tReadingsMap[cAngle] = fReading;
00124       /* Long range [1] */
00125       cAngle += CRadians::PI_OVER_TWO;
00126       cAngle.SignedNormalize();
00127       fReading = CalculateReadingForRay(m_cLongRangeRays1[0], LONG_RANGE_MIN_DISTANCE);
00128       m_tLongReadingsMap[cAngle] = fReading;
00129       m_tReadingsMap[cAngle] = fReading;
00130       /* Short range [2] */
00131       cAngle += CRadians::PI_OVER_TWO;
00132       cAngle.SignedNormalize();
00133       fReading = CalculateReadingForRay(m_cShortRangeRays2[0], SHORT_RANGE_MIN_DISTANCE);
00134       m_tShortReadingsMap[cAngle] = fReading;
00135       m_tReadingsMap[cAngle] = fReading;
00136       /* Long range [3] */
00137       cAngle += CRadians::PI_OVER_TWO;
00138       cAngle.SignedNormalize();
00139       fReading = CalculateReadingForRay(m_cLongRangeRays3[0], LONG_RANGE_MIN_DISTANCE);
00140       m_tLongReadingsMap[cAngle] = fReading;
00141       m_tReadingsMap[cAngle] = fReading;
00142    }
00143 
00144    /****************************************/
00145    /****************************************/
00146 
00147 #define ADD_READING(RAYS,MAP,INDEX,MINDIST)                 \
00148    cAngle += cInterSensorSpan;                              \
00149    cAngle.SignedNormalize();                                \
00150    fReading = CalculateReadingForRay(RAYS[INDEX],MINDIST);  \
00151    MAP[cAngle] = fReading;                                  \
00152    m_tReadingsMap[cAngle] = fReading;
00153 
00154 #define ADD_READINGS(RAYS,MAP,MINDIST)          \
00155    ADD_READING(RAYS,MAP,1,MINDIST)              \
00156    ADD_READING(RAYS,MAP,2,MINDIST)              \
00157    ADD_READING(RAYS,MAP,3,MINDIST)              \
00158    ADD_READING(RAYS,MAP,4,MINDIST)              \
00159    ADD_READING(RAYS,MAP,5,MINDIST)
00160 
00161    void CFootBotDistanceScannerRotZOnlySensor::UpdateRotating() {
00162       CRadians cInterSensorSpan = (m_pcDistScanEntity->GetRotation() - m_cLastDistScanRotation).UnsignedNormalize() / 6.0f;
00163       CRadians cStartAngle = m_cLastDistScanRotation;
00164       /* Short range [0] */
00165       CRadians cAngle = cStartAngle;
00166       cAngle.SignedNormalize();
00167       Real fReading = CalculateReadingForRay(m_cShortRangeRays0[0], SHORT_RANGE_MIN_DISTANCE);
00168       m_tShortReadingsMap[cAngle] = fReading;
00169       m_tReadingsMap[cAngle] = fReading;
00170       ADD_READINGS(m_cShortRangeRays0, m_tShortReadingsMap, SHORT_RANGE_MIN_DISTANCE);
00171       /* Short range [2] */
00172       cAngle = cStartAngle + CRadians::PI;
00173       cAngle.SignedNormalize();
00174       fReading = CalculateReadingForRay(m_cShortRangeRays2[0], SHORT_RANGE_MIN_DISTANCE);
00175       m_tShortReadingsMap[cAngle] = fReading;
00176       m_tReadingsMap[cAngle] = fReading;
00177       ADD_READINGS(m_cShortRangeRays2, m_tShortReadingsMap, SHORT_RANGE_MIN_DISTANCE);
00178       /* Long range [1] */
00179       cAngle = cStartAngle + CRadians::PI_OVER_TWO;
00180       cAngle.SignedNormalize();
00181       fReading = CalculateReadingForRay(m_cLongRangeRays1[0], LONG_RANGE_MIN_DISTANCE);
00182       m_tLongReadingsMap[cAngle] = fReading;
00183       m_tReadingsMap[cAngle] = fReading;
00184       ADD_READINGS(m_cLongRangeRays1, m_tLongReadingsMap, LONG_RANGE_MIN_DISTANCE);
00185       /* Long range [3] */
00186       cAngle = cStartAngle + CRadians::PI_OVER_TWO + CRadians::PI;
00187       cAngle.SignedNormalize();
00188       fReading = CalculateReadingForRay(m_cLongRangeRays3[0], LONG_RANGE_MIN_DISTANCE);
00189       m_tLongReadingsMap[cAngle] = fReading;
00190       m_tReadingsMap[cAngle] = fReading;
00191       ADD_READINGS(m_cLongRangeRays3, m_tLongReadingsMap, LONG_RANGE_MIN_DISTANCE);
00192    }
00193 
00194    /****************************************/
00195    /****************************************/
00196 
00197    Real CFootBotDistanceScannerRotZOnlySensor::CalculateReadingForRay(const CRay3& c_ray,
00198                                                                       Real f_min_distance) {
00199       /* Get the closest intersection */
00200       SEmbodiedEntityIntersectionItem sIntersection;
00201       if(GetClosestEmbodiedEntityIntersectedByRay(sIntersection,
00202                                                   c_ray,
00203                                                   *m_pcEmbodiedEntity)) {
00204          if(m_bShowRays) m_pcControllableEntity->AddIntersectionPoint(c_ray, sIntersection.TOnRay);
00205          /* There is an intersection! */
00206          Real fDistance = c_ray.GetDistance(sIntersection.TOnRay);
00207          if(fDistance > f_min_distance) {
00208             /* The distance is returned in meters, but the reading must be in cm */
00209             if(m_bShowRays) m_pcControllableEntity->AddCheckedRay(true, c_ray);
00210             return fDistance * 100.0f;
00211          }
00212          else {
00213             /* The detected intersection was too close */
00214             if(m_bShowRays) m_pcControllableEntity->AddCheckedRay(true, c_ray);
00215             return -1.0f;
00216          }
00217       }
00218       else {
00219          /* No intersection */
00220          if(m_bShowRays) m_pcControllableEntity->AddCheckedRay(false, c_ray);
00221          return -2.0f;
00222       }
00223    }
00224 
00225    /****************************************/
00226    /****************************************/
00227 
00228 /* Highly reuse the vectors to speed up the computation */
00229 #define CALCULATE_SHORT_RANGE_RAY(ANGLE,INDEX)              \
00230    m_cDirection.RotateZ(ANGLE);                             \
00231    m_cOriginRayStart = m_cDirection;                        \
00232    m_cOriginRayEnd = m_cDirection;                          \
00233    m_cOriginRayStart *= SHORT_RANGE_RAY_START;              \
00234    m_cOriginRayEnd *= SHORT_RANGE_RAY_END;                  \
00235    m_cRayStart = m_pcEmbodiedEntity->GetPosition();         \
00236    m_cRayStart += m_cOriginRayStart;                        \
00237    m_cRayStart.SetZ(m_cRayStart.GetZ() + SENSOR_ELEVATION); \
00238    m_cRayEnd = m_pcEmbodiedEntity->GetPosition();           \
00239    m_cRayEnd += m_cOriginRayEnd;                            \
00240    m_cRayEnd.SetZ(m_cRayEnd.GetZ() + SENSOR_ELEVATION);     \
00241    m_cShortRangeRays0[INDEX].Set(m_cRayStart, m_cRayEnd);   \
00242    m_cRayStart = m_pcEmbodiedEntity->GetPosition();         \
00243    m_cRayStart -= m_cOriginRayStart;                        \
00244    m_cRayStart.SetZ(m_cRayStart.GetZ() + SENSOR_ELEVATION); \
00245    m_cRayEnd = m_pcEmbodiedEntity->GetPosition();           \
00246    m_cRayEnd -= m_cOriginRayEnd;                            \
00247    m_cRayEnd.SetZ(m_cRayEnd.GetZ() + SENSOR_ELEVATION);     \
00248    m_cShortRangeRays2[INDEX].Set(m_cRayStart, m_cRayEnd);   \
00249                                                             \
00250 /* Highly reuse the vectors to speed up the computation */
00251 #define CALCULATE_LONG_RANGE_RAY(ANGLE,INDEX)               \
00252    m_cDirection.RotateZ(ANGLE);                             \
00253    m_cOriginRayStart = m_cDirection;                        \
00254    m_cOriginRayEnd = m_cDirection;                          \
00255    m_cOriginRayStart *= LONG_RANGE_RAY_START;               \
00256    m_cOriginRayEnd *= LONG_RANGE_RAY_END;                   \
00257    m_cRayStart = m_pcEmbodiedEntity->GetPosition();         \
00258    m_cRayStart += m_cOriginRayStart;                        \
00259    m_cRayStart.SetZ(m_cRayStart.GetZ() + SENSOR_ELEVATION); \
00260    m_cRayEnd = m_pcEmbodiedEntity->GetPosition();           \
00261    m_cRayEnd += m_cOriginRayEnd;                            \
00262    m_cRayEnd.SetZ(m_cRayEnd.GetZ() + SENSOR_ELEVATION);     \
00263    m_cLongRangeRays1[INDEX].Set(m_cRayStart, m_cRayEnd);    \
00264    m_cRayStart = m_pcEmbodiedEntity->GetPosition();         \
00265    m_cRayStart -= m_cOriginRayStart;                        \
00266    m_cRayStart.SetZ(m_cRayStart.GetZ() + SENSOR_ELEVATION); \
00267    m_cRayEnd = m_pcEmbodiedEntity->GetPosition();           \
00268    m_cRayEnd -= m_cOriginRayEnd;                            \
00269    m_cRayEnd.SetZ(m_cRayEnd.GetZ() + SENSOR_ELEVATION);     \
00270    m_cLongRangeRays3[INDEX].Set(m_cRayStart, m_cRayEnd);
00271 
00272    /****************************************/
00273    /****************************************/
00274 
00275    void CFootBotDistanceScannerRotZOnlySensor::CalculateRaysNotRotating() {
00276       /* We make the assumption that the foot-bot is rotated only around Z */
00277       /* Get the foot-bot orientation */
00278       CRadians cTmp1, cTmp2, cOrientationZ;
00279       m_pcEmbodiedEntity->GetOrientation().ToEulerAngles(cOrientationZ, cTmp1, cTmp2);
00280       /* Sum the distance scanner orientation */
00281       cOrientationZ += m_pcDistScanEntity->GetRotation();
00282       /* Calculate the 2D vector representing this rotation */
00283       CVector2 cAbsoluteOrientation(1.0, cOrientationZ);
00284       /* The short range sensors are oriented along the foot-bot local X */
00285       m_cDirection = CVector3::X;
00286       CALCULATE_SHORT_RANGE_RAY(cAbsoluteOrientation, 0);
00287       /* The short range sensors are oriented along the foot-bot local Y */
00288       m_cDirection = CVector3::Y;
00289       CALCULATE_LONG_RANGE_RAY(cAbsoluteOrientation, 0);
00290    }
00291 
00292    /****************************************/
00293    /****************************************/
00294 
00295    void CFootBotDistanceScannerRotZOnlySensor::CalculateRaysRotating() {
00296       /* We make the assumption that the foot-bot is rotated only around Z */
00297       /* Get the foot-bot orientation */
00298       CRadians cTmp1, cTmp2, cOrientationZ;
00299       m_pcEmbodiedEntity->GetOrientation().ToEulerAngles(cOrientationZ, cTmp1, cTmp2);
00300       /* Sum the distance scanner orientation */
00301       cOrientationZ += m_cLastDistScanRotation;
00302       /* Calculate the 2D vector representing this rotation */
00303       CVector2 cAbsoluteOrientation(1.0, cOrientationZ);
00304       /* The sensor is rotating, so calculate the span between each successive ray */
00305       CVector2 cInterSensorSpan(1.0f, (m_pcDistScanEntity->GetRotation() - m_cLastDistScanRotation).UnsignedNormalize() / 6.0f);
00306       /* The short range sensors are oriented along the foot-bot local X */
00307       m_cDirection = CVector3::X;
00308       CALCULATE_SHORT_RANGE_RAY(cAbsoluteOrientation, 0);
00309       CALCULATE_SHORT_RANGE_RAY(cInterSensorSpan, 1);
00310       CALCULATE_SHORT_RANGE_RAY(cInterSensorSpan, 2);
00311       CALCULATE_SHORT_RANGE_RAY(cInterSensorSpan, 3);
00312       CALCULATE_SHORT_RANGE_RAY(cInterSensorSpan, 4);
00313       CALCULATE_SHORT_RANGE_RAY(cInterSensorSpan, 5);
00314       /* The long range sensors are oriented along the foot-bot local Y */
00315       m_cDirection = CVector3::Y;
00316       CALCULATE_LONG_RANGE_RAY(cAbsoluteOrientation, 0);
00317       CALCULATE_LONG_RANGE_RAY(cInterSensorSpan, 1);
00318       CALCULATE_LONG_RANGE_RAY(cInterSensorSpan, 2);
00319       CALCULATE_LONG_RANGE_RAY(cInterSensorSpan, 3);
00320       CALCULATE_LONG_RANGE_RAY(cInterSensorSpan, 4);
00321       CALCULATE_LONG_RANGE_RAY(cInterSensorSpan, 5);
00322    }
00323 
00324    /****************************************/
00325    /****************************************/
00326 
00327    REGISTER_SENSOR(CFootBotDistanceScannerRotZOnlySensor,
00328                    "footbot_distance_scanner", "rot_z_only",
00329                    "Carlo Pinciroli [ilpincy@gmail.com]",
00330                    "1.0",
00331                    "The foot-bot distance scanner sensor (optimized for 2D).",
00332                    "This sensor accesses the foot-bot distance scanner sensor. For a complete\n"
00333                    "description of its usage, refer to the common interface.\n"
00334                    "In this implementation, the readings are calculated under the assumption that\n"
00335                    "the foot-bot is always parallel to the XY plane, i.e., it rotates only around\n"
00336                    "the Z axis. This implementation is faster than a 3D one and should be used\n"
00337                    "only when the assumption about the foot-bot rotation holds.\n\n"
00338                    "REQUIRED XML CONFIGURATION\n\n"
00339                    "  <controllers>\n"
00340                    "    ...\n"
00341                    "    <my_controller ...>\n"
00342                    "      ...\n"
00343                    "      <sensors>\n"
00344                    "        ...\n"
00345                    "        <footbot_distance_scanner implementation=\"rot_z_only\" />\n"
00346                    "        ...\n"
00347                    "      </sensors>\n"
00348                    "      ...\n"
00349                    "    </my_controller>\n"
00350                    "    ...\n"
00351                    "  </controllers>\n\n"
00352                    "OPTIONAL XML CONFIGURATION\n\n"
00353                    "It is possible to draw the rays shot by the distance scanner in the OpenGL\n"
00354                    "visualization. This can be useful for sensor debugging but also to understand\n"
00355                    "what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n"
00356                    "they are not obstructed and in purple when they are. In case a ray is\n"
00357                    "obstructed, a black dot is drawn where the intersection occurred.\n"
00358                    "To turn this functionality on, add the attribute 'show_rays=\"true\"' in the\n"
00359                    "XML as in this example:\n\n"
00360                    "  <controllers>\n"
00361                    "    ...\n"
00362                    "    <my_controller ...>\n"
00363                    "      ...\n"
00364                    "      <sensors>\n"
00365                    "        ...\n"
00366                    "        <footbot_distance_scanner implementation=\"rot_z_only\"\n"
00367                    "                                  show_rays=\"true\" />\n"
00368                    "        ...\n"
00369                    "      </sensors>\n"
00370                    "      ...\n"
00371                    "    </my_controller>\n"
00372                    "    ...\n"
00373                    "  </controllers>\n",
00374                    "Usable"
00375       );
00376 
00377 }