ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
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 }