ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/sensors/colored_blob_omnidirectional_camera_rotzonly_sensor.cpp
Go to the documentation of this file.
00001 #include "colored_blob_omnidirectional_camera_rotzonly_sensor.h"
00002 #include <argos3/core/simulator/simulator.h>
00003 #include <argos3/core/simulator/space/positional_indices/positional_index.h>
00004 #include <argos3/core/simulator/entity/composable_entity.h>
00005 #include <argos3/core/simulator/entity/embodied_entity.h>
00006 #include <argos3/plugins/simulator/entities/led_entity.h>
00007 #include <argos3/plugins/simulator/entities/omnidirectional_camera_equipped_entity.h>
00008 #include <argos3/plugins/simulator/media/led_medium.h>
00009 
00010 namespace argos {
00011 
00012    /****************************************/
00013    /****************************************/
00014 
00015    class CLEDCheckOperation : public CPositionalIndex<CLEDEntity>::COperation {
00016 
00017    public:
00018 
00019       CLEDCheckOperation(CCI_ColoredBlobOmnidirectionalCameraSensor::TBlobList& t_blobs,
00020                          COmnidirectionalCameraEquippedEntity& c_omnicam_entity,
00021                          CEmbodiedEntity& c_embodied_entity,
00022                          CControllableEntity& c_controllable_entity,
00023                          bool b_show_rays) :
00024          m_tBlobs(t_blobs),
00025          m_cOmnicamEntity(c_omnicam_entity),
00026          m_cEmbodiedEntity(c_embodied_entity),
00027          m_cControllableEntity(c_controllable_entity),
00028          m_bShowRays(b_show_rays) {
00029          m_pcRootSensingEntity = &m_cEmbodiedEntity.GetParent();
00030       }
00031       virtual ~CLEDCheckOperation() {
00032          while(! m_tBlobs.empty()) {
00033             delete m_tBlobs.back();
00034             m_tBlobs.pop_back();
00035          }
00036       }
00037 
00038       virtual bool operator()(CLEDEntity& c_led) {
00039          /* Process this LED only if it's lit */
00040          if(c_led.GetColor() != CColor::BLACK) {
00041             if(c_led.HasParent()) {
00042                /* Filter out the LEDs belonging to the sensing entity by checking if they share the same parent entity */
00043                m_pcRootOfLEDEntity = &c_led.GetParent();
00044                while(m_pcRootOfLEDEntity->HasParent()) m_pcRootOfLEDEntity = &m_pcRootOfLEDEntity->GetParent();
00045                if(m_pcRootSensingEntity == m_pcRootOfLEDEntity) {
00046                   return true;
00047                }
00048             }
00049             /* If we are here, it's because the LED must be processed */
00050             m_cOcclusionCheckRay.SetEnd(c_led.GetPosition());
00051             m_cLEDRelativePos = c_led.GetPosition();
00052             m_cLEDRelativePos -= m_cCameraPos;
00053             m_cLEDRelativePosXY.Set(m_cLEDRelativePos.GetX(),
00054                                     m_cLEDRelativePos.GetY());
00055             if(Abs(m_cLEDRelativePos.GetX()) < m_fGroundHalfRange &&
00056                Abs(m_cLEDRelativePos.GetY()) < m_fGroundHalfRange &&
00057                m_cLEDRelativePos.GetZ() < m_cCameraPos.GetZ() &&
00058                !GetClosestEmbodiedEntityIntersectedByRay(m_sIntersectionItem,
00059                                                          m_cOcclusionCheckRay,
00060                                                          m_cEmbodiedEntity)) {
00061                m_tBlobs.push_back(new CCI_ColoredBlobOmnidirectionalCameraSensor::SBlob(c_led.GetColor(),
00062                                                                                         m_cLEDRelativePosXY.Angle() - m_cCameraOrient,
00063                                                                                         m_cLEDRelativePosXY.Length() * 100.0f));
00064                if(m_bShowRays) {
00065                   m_cControllableEntity.AddCheckedRay(false, CRay3(m_cCameraPos, c_led.GetPosition()));
00066                }
00067             }
00068          }
00069          return true;
00070       }
00071 
00072       void Setup(Real f_ground_half_range) {
00073          while(! m_tBlobs.empty()) {
00074             delete m_tBlobs.back();
00075             m_tBlobs.pop_back();
00076          }
00077          m_fGroundHalfRange = f_ground_half_range;
00078          m_cEmbodiedEntity.GetOrientation().ToEulerAngles(m_cCameraOrient, m_cTmp1, m_cTmp2);
00079          m_cCameraPos = m_cEmbodiedEntity.GetPosition();
00080          m_cCameraPos += m_cOmnicamEntity.GetOffset();
00081          m_cOcclusionCheckRay.SetStart(m_cCameraPos);
00082       }
00083       
00084    private:
00085       
00086       CCI_ColoredBlobOmnidirectionalCameraSensor::TBlobList& m_tBlobs;
00087       COmnidirectionalCameraEquippedEntity& m_cOmnicamEntity;
00088       CEmbodiedEntity& m_cEmbodiedEntity;
00089       CControllableEntity& m_cControllableEntity;
00090       Real m_fGroundHalfRange;
00091       bool m_bShowRays;
00092       CEntity* m_pcRootSensingEntity;
00093       CEntity* m_pcRootOfLEDEntity;
00094       CVector3 m_cCameraPos;
00095       CRadians m_cCameraOrient;
00096       CRadians m_cTmp1, m_cTmp2;
00097       CVector3 m_cLEDRelativePos;
00098       CVector2 m_cLEDRelativePosXY;
00099       SEmbodiedEntityIntersectionItem m_sIntersectionItem;
00100       CRay3 m_cOcclusionCheckRay;
00101    };
00102 
00103    /****************************************/
00104    /****************************************/
00105 
00106    CColoredBlobOmnidirectionalCameraRotZOnlySensor::CColoredBlobOmnidirectionalCameraRotZOnlySensor() :
00107       m_bEnabled(false),
00108       m_pcOmnicamEntity(NULL),
00109       m_pcControllableEntity(NULL),
00110       m_pcEmbodiedEntity(NULL),
00111       m_pcLEDIndex(NULL),
00112       m_pcEmbodiedIndex(NULL),
00113       m_fDistanceNoiseStdDev(0.0f),
00114       m_pcRNG(NULL),
00115       m_bShowRays(false) {}
00116 
00117    /****************************************/
00118    /****************************************/
00119 
00120    CColoredBlobOmnidirectionalCameraRotZOnlySensor::~CColoredBlobOmnidirectionalCameraRotZOnlySensor() {
00121    }
00122 
00123    /****************************************/
00124    /****************************************/
00125 
00126    void CColoredBlobOmnidirectionalCameraRotZOnlySensor::SetRobot(CComposableEntity& c_entity) {
00127       /* Get and enable omndirectional camera equipped entity */
00128       m_pcOmnicamEntity = &(c_entity.GetComponent<COmnidirectionalCameraEquippedEntity>("omnidirectional_camera"));
00129       m_pcOmnicamEntity->SetCanBeEnabledIfDisabled(true);
00130       /* Get controllable entity */
00131       m_pcControllableEntity = &(c_entity.GetComponent<CControllableEntity>("controller"));
00132       /* Get embodied entity */
00133       m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body"));
00134    }
00135 
00136    /****************************************/
00137    /****************************************/
00138 
00139    void CColoredBlobOmnidirectionalCameraRotZOnlySensor::Init(TConfigurationNode& t_tree) {
00140       try {
00141          /* Parent class init */
00142          CCI_ColoredBlobOmnidirectionalCameraSensor::Init(t_tree);
00143          /* Show rays? */
00144          GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
00145          /* Parse noise */
00146          GetNodeAttributeOrDefault(t_tree, "noise_std_dev", m_fDistanceNoiseStdDev, m_fDistanceNoiseStdDev);
00147          if(m_fDistanceNoiseStdDev > 0.0f) {
00148             m_pcRNG = CRandom::CreateRNG("argos");
00149          }
00150          /* Get LED medium from id specified in the XML */
00151          std::string strMedium;
00152          GetNodeAttribute(t_tree, "medium", strMedium);
00153          m_pcLEDIndex = &(CSimulator::GetInstance().GetMedium<CLEDMedium>(strMedium).GetIndex());
00154          /* Create check operation */
00155          m_pcOperation = new CLEDCheckOperation(m_sReadings.BlobList,
00156                                                 *m_pcOmnicamEntity,
00157                                                 *m_pcEmbodiedEntity,
00158                                                 *m_pcControllableEntity,
00159                                                 m_bShowRays);
00160       }
00161       catch(CARGoSException& ex) {
00162          THROW_ARGOSEXCEPTION_NESTED("Error initializing the colored blob omnidirectional camera rotzonly sensor", ex);
00163       }
00164    }
00165 
00166    /****************************************/
00167    /****************************************/
00168 
00169    void CColoredBlobOmnidirectionalCameraRotZOnlySensor::Update() {
00170       if(m_bEnabled) {
00171          /* Increase data counter */
00172          ++m_sReadings.Counter;
00173          /* Calculate range on the ground */
00174          CVector3 cCameraPos = m_pcOmnicamEntity->GetOffset();
00175          cCameraPos += m_pcEmbodiedEntity->GetPosition();
00176          Real fGroundHalfRange = cCameraPos.GetZ() * Tan(m_pcOmnicamEntity->GetAperture());
00177          /* Prepare the operation */
00178          m_pcOperation->Setup(fGroundHalfRange);
00179          /* Go through LED entities in box range */
00180          m_pcLEDIndex->ForEntitiesInBoxRange(
00181             CVector3(cCameraPos.GetX(),
00182                      cCameraPos.GetY(),
00183                      cCameraPos.GetZ() * 0.5f),
00184             CVector3(fGroundHalfRange, fGroundHalfRange, cCameraPos.GetZ() * 0.5f),
00185             *m_pcOperation);
00186       }
00187    }
00188 
00189    /****************************************/
00190    /****************************************/
00191 
00192    void CColoredBlobOmnidirectionalCameraRotZOnlySensor::Reset() {
00193       m_sReadings.Counter = 0;
00194       m_sReadings.BlobList.clear();
00195    }
00196 
00197    /****************************************/
00198    /****************************************/
00199 
00200    void CColoredBlobOmnidirectionalCameraRotZOnlySensor::Destroy() {
00201       delete m_pcOperation;
00202    }
00203 
00204    /****************************************/
00205    /****************************************/
00206 
00207    void CColoredBlobOmnidirectionalCameraRotZOnlySensor::Enable() {
00208       m_pcOmnicamEntity->Enable();
00209       m_bEnabled = true;
00210    }
00211 
00212    /****************************************/
00213    /****************************************/
00214 
00215    void CColoredBlobOmnidirectionalCameraRotZOnlySensor::Disable() {
00216       m_pcOmnicamEntity->Disable();
00217       m_bEnabled = false;
00218    }
00219 
00220    /****************************************/
00221    /****************************************/
00222 
00223    REGISTER_SENSOR(CColoredBlobOmnidirectionalCameraRotZOnlySensor,
00224                    "colored_blob_omnidirectional_camera", "rot_z_only",
00225                    "Carlo Pinciroli [ilpincy@gmail.com]",
00226                    "1.0",
00227                    "A generic omnidirectional camera sensor to detect colored blobs.",
00228                    "TODO\n\n",
00229                    "Usable"
00230                   );
00231 
00232 }